From e2f8c89f1e45e0a34b31a0c052c36424905e1ca5 Mon Sep 17 00:00:00 2001 From: Bernd Pfrommer Date: Mon, 17 Jun 2024 15:06:32 -0400 Subject: [PATCH 1/5] reformatted vnproglib to pass clang-format test --- .../cpp/include/vn/attitude.h | 114 +- .../cpp/include/vn/boostpython.h | 20 +- .../cpp/include/vn/compiler.h | 20 +- .../cpp/include/vn/compositedata.h | 1055 +-- .../vnproglib-1.2.0.0/cpp/include/vn/consts.h | 10 +- .../cpp/include/vn/conversions.h | 26 +- .../cpp/include/vn/criticalsection.h | 45 +- .../cpp/include/vn/dllvalidator.h | 58 +- .../cpp/include/vn/error_detection.h | 53 +- .../vnproglib-1.2.0.0/cpp/include/vn/event.h | 95 +- .../cpp/include/vn/exceptions.h | 50 +- .../vnproglib-1.2.0.0/cpp/include/vn/export.h | 24 +- .../cpp/include/vn/ezasyncdata.h | 90 +- .../vnproglib-1.2.0.0/cpp/include/vn/int.h | 24 +- .../vnproglib-1.2.0.0/cpp/include/vn/matrix.h | 2654 ++++---- .../cpp/include/vn/memoryport.h | 108 +- .../vnproglib-1.2.0.0/cpp/include/vn/mock.h | 8 +- .../vnproglib-1.2.0.0/cpp/include/vn/nocopy.h | 30 +- .../vnproglib-1.2.0.0/cpp/include/vn/packet.h | 4618 +++++++------ .../cpp/include/vn/packetfinder.h | 152 +- .../vnproglib-1.2.0.0/cpp/include/vn/port.h | 121 +- .../cpp/include/vn/position.h | 51 +- .../cpp/include/vn/registers.h | 2406 +++---- .../cpp/include/vn/rtcmlistener.h | 148 +- .../cpp/include/vn/rtcmmessage.h | 141 +- .../cpp/include/vn/searcher.h | 91 +- .../cpp/include/vn/sensors.h | 2964 ++++---- .../cpp/include/vn/serialport.h | 212 +- .../vnproglib-1.2.0.0/cpp/include/vn/signal.h | 141 +- .../vnproglib-1.2.0.0/cpp/include/vn/thread.h | 131 +- .../vnproglib-1.2.0.0/cpp/include/vn/types.h | 967 ++- .../vnproglib-1.2.0.0/cpp/include/vn/util.h | 79 +- .../cpp/include/vn/utilities.h | 83 +- .../vnproglib-1.2.0.0/cpp/include/vn/vector.h | 2044 +++--- .../vnproglib-1.2.0.0/cpp/include/vn/vntime.h | 61 +- .../vnproglib-1.2.0.0/cpp/src/attitude.cpp | 153 +- .../vnproglib-1.2.0.0/cpp/src/benchmark.cpp | 22 +- .../cpp/src/compositedata.cpp | 2999 ++++----- .../vnproglib-1.2.0.0/cpp/src/conversions.cpp | 414 +- .../cpp/src/criticalsection.cpp | 95 +- .../cpp/src/dllvalidator.cpp | 153 +- .../cpp/src/error_detection.cpp | 50 +- vectornav/vnproglib-1.2.0.0/cpp/src/event.cpp | 334 +- .../vnproglib-1.2.0.0/cpp/src/ezasyncdata.cpp | 138 +- .../vnproglib-1.2.0.0/cpp/src/memoryport.cpp | 189 +- .../vnproglib-1.2.0.0/cpp/src/packet.cpp | 5982 +++++++++-------- .../cpp/src/packetfinder.cpp | 932 ++- vectornav/vnproglib-1.2.0.0/cpp/src/port.cpp | 12 +- .../vnproglib-1.2.0.0/cpp/src/position.cpp | 25 +- .../cpp/src/rtcmlistener.cpp | 462 +- .../vnproglib-1.2.0.0/cpp/src/rtcmmessage.cpp | 583 +- .../vnproglib-1.2.0.0/cpp/src/searcher.cpp | 273 +- .../vnproglib-1.2.0.0/cpp/src/sensors.cpp | 4600 ++++++------- .../vnproglib-1.2.0.0/cpp/src/serialport.cpp | 2150 +++--- .../vnproglib-1.2.0.0/cpp/src/thread.cpp | 249 +- vectornav/vnproglib-1.2.0.0/cpp/src/types.cpp | 65 +- vectornav/vnproglib-1.2.0.0/cpp/src/util.cpp | 1108 ++- .../vnproglib-1.2.0.0/cpp/src/utilities.cpp | 250 +- .../vnproglib-1.2.0.0/cpp/src/vntime.cpp | 300 +- 59 files changed, 19383 insertions(+), 21049 deletions(-) diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/attitude.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/attitude.h index 51612681..e2594622 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/attitude.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/attitude.h @@ -1,86 +1,78 @@ #ifndef _VNMATH_ATTITUDE_H_ #define _VNMATH_ATTITUDE_H_ -#include "vector.h" -#include "matrix.h" #include "export.h" +#include "matrix.h" +#include "vector.h" -namespace vn { -namespace math { +namespace vn +{ +namespace math +{ /// \brief Representation of an orientation/attitude. class vn_proglib_DLLEXPORT AttitudeF { private: - - enum AttitudeType - { - ATT_YprRads, - ATT_YprDegs, - ATT_Quat, - ATT_Dcm - }; + enum AttitudeType { ATT_YprRads, ATT_YprDegs, ATT_Quat, ATT_Dcm }; public: - - /// \brief Returns an AttitudeF representing no rotation. - static AttitudeF noRotation(); - - /// \brief Creates a new AttitudeF from a quaternion. - /// - /// \param[in] quat The orientation expressed as a quaternion. - /// \return The new AttitudeF. - static AttitudeF fromQuat(vec4f quat); - - /// \brief Creates a new AttitudeF from a yaw, pitch, roll in degrees. - /// - /// \param[in] yprInDegs The yaw, pitch, roll in degrees. - /// \return The new AttitudeF. - static AttitudeF fromYprInDegs(vec3f yprInDegs); - - /// \brief Creates a new AttitudeF from a yaw, pitch, roll in radians. - /// - /// \param[in] yprInRads The yaw, pitch, roll in radians. - /// \return The new AttitudeF. - static AttitudeF fromYprInRads(vec3f yprInRads); - - /// \brief Creates a new AttitudeF from a direction cosine matrix. - /// - /// \param[in] dcm The direction cosine matrix. - /// \return The new AttitudeF. - static AttitudeF fromDcm(mat3f dcm); - - // TEMP - AttitudeF() { } + /// \brief Returns an AttitudeF representing no rotation. + static AttitudeF noRotation(); + + /// \brief Creates a new AttitudeF from a quaternion. + /// + /// \param[in] quat The orientation expressed as a quaternion. + /// \return The new AttitudeF. + static AttitudeF fromQuat(vec4f quat); + + /// \brief Creates a new AttitudeF from a yaw, pitch, roll in degrees. + /// + /// \param[in] yprInDegs The yaw, pitch, roll in degrees. + /// \return The new AttitudeF. + static AttitudeF fromYprInDegs(vec3f yprInDegs); + + /// \brief Creates a new AttitudeF from a yaw, pitch, roll in radians. + /// + /// \param[in] yprInRads The yaw, pitch, roll in radians. + /// \return The new AttitudeF. + static AttitudeF fromYprInRads(vec3f yprInRads); + + /// \brief Creates a new AttitudeF from a direction cosine matrix. + /// + /// \param[in] dcm The direction cosine matrix. + /// \return The new AttitudeF. + static AttitudeF fromDcm(mat3f dcm); + + // TEMP + AttitudeF() {} private: - - AttitudeF(AttitudeType type, void* attitude); + AttitudeF(AttitudeType type, void * attitude); public: + /// \brief Returns the orientation as represented in yaw, pitch, roll in degrees. + /// \return The orientation in yaw, pitch, roll in degrees. + vec3f yprInDegs(); - /// \brief Returns the orientation as represented in yaw, pitch, roll in degrees. - /// \return The orientation in yaw, pitch, roll in degrees. - vec3f yprInDegs(); - - /// \brief Returns the orientation as represented in yaw, pitch, roll in radians. - /// \return The orientation in yaw, pitch, roll in radians. - vec3f yprInRads(); + /// \brief Returns the orientation as represented in yaw, pitch, roll in radians. + /// \return The orientation in yaw, pitch, roll in radians. + vec3f yprInRads(); - /// \brief Returns the orientation as represented in quaternion. - /// \return The orientation in quaternion. - vec4f quat(); + /// \brief Returns the orientation as represented in quaternion. + /// \return The orientation in quaternion. + vec4f quat(); - /// \brief Returns the orientation as represented by a direction cosine matrix. - /// \return The orientation as a direction cosine matrix. - mat3f dcm(); + /// \brief Returns the orientation as represented by a direction cosine matrix. + /// \return The orientation as a direction cosine matrix. + mat3f dcm(); private: - AttitudeType _underlyingType; - uint8_t _data[sizeof(mat3f)]; + AttitudeType _underlyingType; + uint8_t _data[sizeof(mat3f)]; }; -} -} +} // namespace math +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/boostpython.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/boostpython.h index 112416e9..8f5fcbd5 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/boostpython.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/boostpython.h @@ -4,20 +4,20 @@ // This header files allows cleanly including the common Python headers. #if defined(_MSC_VER) - #pragma warning(push) - #pragma warning(disable:4100) - #pragma warning(disable:4121) - #pragma warning(disable:4127) - #pragma warning(disable:4244) - #pragma warning(disable:4510) - #pragma warning(disable:4512) - #pragma warning(disable:4610) +#pragma warning(push) +#pragma warning(disable : 4100) +#pragma warning(disable : 4121) +#pragma warning(disable : 4127) +#pragma warning(disable : 4244) +#pragma warning(disable : 4510) +#pragma warning(disable : 4512) +#pragma warning(disable : 4610) #endif #include "boost/python.hpp" -#if defined (_MSC_VER) - #pragma warning(pop) +#if defined(_MSC_VER) +#pragma warning(pop) #endif #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/compiler.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/compiler.h index 89e96c6a..a7477e5a 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/compiler.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/compiler.h @@ -26,9 +26,9 @@ // #endif // #if (defined(_MSC_VER) && _MSC_VER > 1500) || (__cplusplus >= 201103L) - #define VN_SUPPORTS_SWAP 1 +#define VN_SUPPORTS_SWAP 1 #else - #define VN_SUPPORTS_SWAP 0 +#define VN_SUPPORTS_SWAP 0 #endif // The VN_SUPPORTS_INITIALIZER_LIST define indicates if the compiler supports @@ -45,9 +45,9 @@ // #endif // #if (defined(_MSC_VER) && _MSC_VER <= 1600) || (__cplusplus < 201103L) - #define VN_SUPPORTS_INITIALIZER_LIST 0 +#define VN_SUPPORTS_INITIALIZER_LIST 0 #else - #define VN_SUPPORTS_INITIALIZER_LIST 1 +#define VN_SUPPORTS_INITIALIZER_LIST 1 #endif // The VN_SUPPORTS_CSTR_STRING_CONCATENATE define indictes if the compiler supports @@ -62,18 +62,18 @@ // #endif // #if (defined(_MSC_VER) && _MSC_VER <= 1600) - #define VN_SUPPORTS_CSTR_STRING_CONCATENATE 0 +#define VN_SUPPORTS_CSTR_STRING_CONCATENATE 0 #else - #define VN_SUPPORTS_CSTR_STRING_CONCATENATE 1 +#define VN_SUPPORTS_CSTR_STRING_CONCATENATE 1 #endif // Determine if the secure CRT and SCL are available. #if defined(_MSC_VER) - #define VN_HAVE_SECURE_CRT 1 - #define VN_HAVE_SECURE_SCL 1 +#define VN_HAVE_SECURE_CRT 1 +#define VN_HAVE_SECURE_SCL 1 #else - #define VN_HAVE_SECURE_CRT 0 - #define VN_HAVE_SECURE_SCL 0 +#define VN_HAVE_SECURE_CRT 0 +#define VN_HAVE_SECURE_SCL 0 #endif #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/compositedata.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/compositedata.h index 90716b81..a2c868cc 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/compositedata.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/compositedata.h @@ -3,293 +3,290 @@ #include +#include "vn/attitude.h" #include "vn/export.h" #include "vn/packet.h" -#include "vn/attitude.h" #include "vn/position.h" -namespace vn { -namespace sensors { +namespace vn +{ +namespace sensors +{ /// \brief Composite structure of all data types available from VectorNav sensors. class vn_proglib_DLLEXPORT CompositeData { public: + CompositeData(); + CompositeData(const CompositeData & cd); + + ~CompositeData(); + + CompositeData & operator=(const CompositeData & RHS); + + /// \brief Parses a packet. + /// + /// \param[in] p The packet to parse. + /// \return The data contained in the parsed packet. + static CompositeData parse(protocol::uart::Packet & p); + + /// \brief Parses a packet. + /// + /// \param[in] p The packet to parse. + /// \param[in/out] o The CompositeData structure to write the data to. + static void parse(protocol::uart::Packet & p, CompositeData & o); + + /// \brief Parses a packet and updates multiple CompositeData objects. + /// + /// \param[in] p The packet to parse. + /// \param[in] o The collection of CompositeData objects to update. + static void parse(protocol::uart::Packet & p, std::vector & o); + + /// \brief Resets the data contained in the CompositeData object. + void reset(); + + /// \brief Indicates if anyAttitude has valid data. + /// \return true if anyAttitude has valid data; otherwise false. + bool hasAnyAttitude(); + + /// \brief Gets and converts the latest attitude data regardless of the received + /// underlying type. Based on which type of attitude data that was last received + /// and processed, this value may be based on either received yaw, pitch, roll, + /// quaternion, or direction cosine matrix data. + /// + /// \return The attitude data. + /// \exception invalid_operation Thrown if there is no valid data. + math::AttitudeF anyAttitude(); + + /// \brief Indicates if yawPitchRoll has valid data. + /// \return true if yawPitchRoll has valid data; otherwise false. + bool hasYawPitchRoll(); + + /// \brief Yaw, pitch, roll data. + /// \return The yaw, pitch, roll data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f yawPitchRoll(); + + /// \brief Indicates if quaternion has valid data. + /// \return true if quaternion has valid data; otherwise false. + bool hasQuaternion(); + + /// \brief Quaternion data. + /// \return Quaternion data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec4f quaternion(); + + /// \brief Indicates if directionCosineMatrix has valid data. + /// \return true if directionCosineMatrix has valid data; otherwise false. + bool hasDirectionCosineMatrix(); + + /// \brief Direction cosine matrix data. + /// \return Direction cosine matrix data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::mat3f directionCosineMatrix(); + + /// \brief Indicates if anyMagnetic has valid data. + /// \return true if anyMagnetic has valid data; otherwise false. + bool hasAnyMagnetic(); + + /// \brief Gets and converts the latest magnetic data regardless of the received + /// underlying type. Based on which type of magnetic data that was last received + /// and processed, this value may be based on either received magnetic or + /// magneticUncompensated data. + /// + /// \return The magnetic data. + /// \exception invalid_operation Thrown if there is no valid data. + math::vec3f anyMagnetic(); + + /// \brief Indicates if magnetic has valid data. + /// \return true if magnetic has valid data; otherwise false. + bool hasMagnetic(); + + /// \brief Magnetic data. + /// \return The magnetic data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f magnetic(); + + /// \brief Indicates if magneticUncompensated has valid data. + /// \return true if magneticUncompensated has valid data; otherwise false. + bool hasMagneticUncompensated(); + + /// \brief Magnetic uncompensated data. + /// \return The magnetic uncompensated data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f magneticUncompensated(); + + /// \brief Indicates if magneticNed has valid data. + /// \return true if magneticNed has valid data; otherwise false. + bool hasMagneticNed(); + + /// \brief Magnetic NED data. + /// \return The magnetic NED data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f magneticNed(); + + /// \brief Indicates if magneticEcef has valid data. + /// \return true if magneticEcef has valid data; otherwise false. + bool hasMagneticEcef(); + + /// \brief Magnetic ECEF data. + /// \return The magnetic ECEF data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f magneticEcef(); + + /// \brief Indicates if anyAcceleration has valid data. + /// \return true if anyAcceleration has valid data; otherwise false. + bool hasAnyAcceleration(); + + /// \brief Gets and converts the latest acceleration data regardless of the received + /// underlying type. + /// + /// \return The acceleration data. + /// \exception invalid_operation Thrown if there is no valid data. + math::vec3f anyAcceleration(); + + /// \brief Indicates if acceleration has valid data. + /// \return true if acceleration has valid data; otherwise false. + bool hasAcceleration(); + + /// \brief Acceleration data. + /// \return The acceleration data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f acceleration(); + + /// \brief Indicates if accelerationLinearBody has valid data. + /// \return true if accelerationLinearBody has valid data; otherwise false. + bool hasAccelerationLinearBody(); + + /// \brief Acceleration linear body data. + /// \return The acceleration linear body data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f accelerationLinearBody(); + + /// \brief Indicates if accelerationUncompensated has valid data. + /// \return true if accelerationUncompensated has valid data; otherwise false. + bool hasAccelerationUncompensated(); + + /// \brief Acceleration uncompensated data. + /// \return The acceleration uncompensated data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f accelerationUncompensated(); + + /// \brief Indicates if accelerationLinearNed has valid data. + /// \return true if accelerationLinearNed has valid data; otherwise false. + bool hasAccelerationLinearNed(); + + /// \brief Acceleration linear NED data. + /// \return The acceleration linear NED data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f accelerationLinearNed(); + + /// \brief Indicates if accelerationLinearEcef has valid data. + /// \return true if accelerationLinearEcef has valid data; otherwise false. + bool hasAccelerationLinearEcef(); + + /// \brief Acceleration linear ECEF data. + /// \return The acceleration linear ECEF data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f accelerationLinearEcef(); + + /// \brief Indicates if accelerationNed has valid data. + /// \return true if accelerationNed has valid data; otherwise false. + bool hasAccelerationNed(); + + /// \brief Acceleration NED data. + /// \return The acceleration NED data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f accelerationNed(); + + /// \brief Indicates if accelerationEcef has valid data. + /// \return true if accelerationEcef has valid data; otherwise false. + bool hasAccelerationEcef(); + + /// \brief Acceleration ECEF data. + /// \return The acceleration ECEF data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f accelerationEcef(); + + /// \brief Indicates if anyAngularRate has valid data. + /// \return true if anyAngularRate has valid data; otherwise false. + bool hasAnyAngularRate(); + + /// \brief Gets and converts the latest angular rate data regardless of the received + /// underlying type. + /// + /// \return The angular rate data. + /// \exception invalid_operation Thrown if there is no valid data. + math::vec3f anyAngularRate(); + + /// \brief Indicates if angularRate has valid data. + /// \return true if angularRate has valid data; otherwise false. + bool hasAngularRate(); + + /// \brief Angular rate data. + /// \return The angular rate data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f angularRate(); + + /// \brief Indicates if angularRateUncompensated has valid data. + /// \return true if angularRateUncompensated has valid data; otherwise false. + bool hasAngularRateUncompensated(); + + /// \brief Angular rate uncompensated data. + /// \return The angular rate uncompensated data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f angularRateUncompensated(); + + /// \brief Indicates if anyTemperature has valid data. + /// \return true if anyTemperature has valid data; otherwise false. + bool hasAnyTemperature(); + + /// \brief Gets and converts the latest temperature data regardless of the received + /// underlying type. + /// + /// \return The tempature data. + /// \exception invalid_operation Thrown if there is no valid data. + float anyTemperature(); + + /// \brief Indicates if temperature has valid data. + /// \return true if temperature has valid data; otherwise false. + bool hasTemperature(); + + /// \brief Temperature data. + /// \return The temperature data. + /// \exception invalid_operation Thrown if there is not any valid data. + float temperature(); - CompositeData(); - CompositeData(const CompositeData& cd); - - ~CompositeData(); - - CompositeData& operator=(const CompositeData& RHS); - - /// \brief Parses a packet. - /// - /// \param[in] p The packet to parse. - /// \return The data contained in the parsed packet. - static CompositeData parse(protocol::uart::Packet& p); - - /// \brief Parses a packet. - /// - /// \param[in] p The packet to parse. - /// \param[in/out] o The CompositeData structure to write the data to. - static void parse(protocol::uart::Packet& p, CompositeData& o); - - /// \brief Parses a packet and updates multiple CompositeData objects. - /// - /// \param[in] p The packet to parse. - /// \param[in] o The collection of CompositeData objects to update. - static void parse(protocol::uart::Packet& p, std::vector& o); - - /// \brief Resets the data contained in the CompositeData object. - void reset(); - - /// \brief Indicates if anyAttitude has valid data. - /// \return true if anyAttitude has valid data; otherwise false. - bool hasAnyAttitude(); - - /// \brief Gets and converts the latest attitude data regardless of the received - /// underlying type. Based on which type of attitude data that was last received - /// and processed, this value may be based on either received yaw, pitch, roll, - /// quaternion, or direction cosine matrix data. - /// - /// \return The attitude data. - /// \exception invalid_operation Thrown if there is no valid data. - math::AttitudeF anyAttitude(); - - /// \brief Indicates if yawPitchRoll has valid data. - /// \return true if yawPitchRoll has valid data; otherwise false. - bool hasYawPitchRoll(); - - /// \brief Yaw, pitch, roll data. - /// \return The yaw, pitch, roll data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f yawPitchRoll(); - - /// \brief Indicates if quaternion has valid data. - /// \return true if quaternion has valid data; otherwise false. - bool hasQuaternion(); - - /// \brief Quaternion data. - /// \return Quaternion data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec4f quaternion(); - - /// \brief Indicates if directionCosineMatrix has valid data. - /// \return true if directionCosineMatrix has valid data; otherwise false. - bool hasDirectionCosineMatrix(); - - /// \brief Direction cosine matrix data. - /// \return Direction cosine matrix data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::mat3f directionCosineMatrix(); - - /// \brief Indicates if anyMagnetic has valid data. - /// \return true if anyMagnetic has valid data; otherwise false. - bool hasAnyMagnetic(); - - /// \brief Gets and converts the latest magnetic data regardless of the received - /// underlying type. Based on which type of magnetic data that was last received - /// and processed, this value may be based on either received magnetic or - /// magneticUncompensated data. - /// - /// \return The magnetic data. - /// \exception invalid_operation Thrown if there is no valid data. - math::vec3f anyMagnetic(); - - /// \brief Indicates if magnetic has valid data. - /// \return true if magnetic has valid data; otherwise false. - bool hasMagnetic(); - - /// \brief Magnetic data. - /// \return The magnetic data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f magnetic(); - - /// \brief Indicates if magneticUncompensated has valid data. - /// \return true if magneticUncompensated has valid data; otherwise false. - bool hasMagneticUncompensated(); - - /// \brief Magnetic uncompensated data. - /// \return The magnetic uncompensated data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f magneticUncompensated(); - - /// \brief Indicates if magneticNed has valid data. - /// \return true if magneticNed has valid data; otherwise false. - bool hasMagneticNed(); - - /// \brief Magnetic NED data. - /// \return The magnetic NED data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f magneticNed(); - - /// \brief Indicates if magneticEcef has valid data. - /// \return true if magneticEcef has valid data; otherwise false. - bool hasMagneticEcef(); - - /// \brief Magnetic ECEF data. - /// \return The magnetic ECEF data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f magneticEcef(); - - - /// \brief Indicates if anyAcceleration has valid data. - /// \return true if anyAcceleration has valid data; otherwise false. - bool hasAnyAcceleration(); - - /// \brief Gets and converts the latest acceleration data regardless of the received - /// underlying type. - /// - /// \return The acceleration data. - /// \exception invalid_operation Thrown if there is no valid data. - math::vec3f anyAcceleration(); - - /// \brief Indicates if acceleration has valid data. - /// \return true if acceleration has valid data; otherwise false. - bool hasAcceleration(); - - /// \brief Acceleration data. - /// \return The acceleration data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f acceleration(); - - /// \brief Indicates if accelerationLinearBody has valid data. - /// \return true if accelerationLinearBody has valid data; otherwise false. - bool hasAccelerationLinearBody(); - - /// \brief Acceleration linear body data. - /// \return The acceleration linear body data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f accelerationLinearBody(); - - /// \brief Indicates if accelerationUncompensated has valid data. - /// \return true if accelerationUncompensated has valid data; otherwise false. - bool hasAccelerationUncompensated(); - - /// \brief Acceleration uncompensated data. - /// \return The acceleration uncompensated data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f accelerationUncompensated(); - - /// \brief Indicates if accelerationLinearNed has valid data. - /// \return true if accelerationLinearNed has valid data; otherwise false. - bool hasAccelerationLinearNed(); - - /// \brief Acceleration linear NED data. - /// \return The acceleration linear NED data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f accelerationLinearNed(); - - /// \brief Indicates if accelerationLinearEcef has valid data. - /// \return true if accelerationLinearEcef has valid data; otherwise false. - bool hasAccelerationLinearEcef(); - - /// \brief Acceleration linear ECEF data. - /// \return The acceleration linear ECEF data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f accelerationLinearEcef(); - - /// \brief Indicates if accelerationNed has valid data. - /// \return true if accelerationNed has valid data; otherwise false. - bool hasAccelerationNed(); - - /// \brief Acceleration NED data. - /// \return The acceleration NED data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f accelerationNed(); - - /// \brief Indicates if accelerationEcef has valid data. - /// \return true if accelerationEcef has valid data; otherwise false. - bool hasAccelerationEcef(); - - /// \brief Acceleration ECEF data. - /// \return The acceleration ECEF data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f accelerationEcef(); - - - /// \brief Indicates if anyAngularRate has valid data. - /// \return true if anyAngularRate has valid data; otherwise false. - bool hasAnyAngularRate(); - - /// \brief Gets and converts the latest angular rate data regardless of the received - /// underlying type. - /// - /// \return The angular rate data. - /// \exception invalid_operation Thrown if there is no valid data. - math::vec3f anyAngularRate(); - - /// \brief Indicates if angularRate has valid data. - /// \return true if angularRate has valid data; otherwise false. - bool hasAngularRate(); - - /// \brief Angular rate data. - /// \return The angular rate data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f angularRate(); - - /// \brief Indicates if angularRateUncompensated has valid data. - /// \return true if angularRateUncompensated has valid data; otherwise false. - bool hasAngularRateUncompensated(); - - /// \brief Angular rate uncompensated data. - /// \return The angular rate uncompensated data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f angularRateUncompensated(); - - - /// \brief Indicates if anyTemperature has valid data. - /// \return true if anyTemperature has valid data; otherwise false. - bool hasAnyTemperature(); - - /// \brief Gets and converts the latest temperature data regardless of the received - /// underlying type. - /// - /// \return The tempature data. - /// \exception invalid_operation Thrown if there is no valid data. - float anyTemperature(); - - /// \brief Indicates if temperature has valid data. - /// \return true if temperature has valid data; otherwise false. - bool hasTemperature(); - - /// \brief Temperature data. - /// \return The temperature data. - /// \exception invalid_operation Thrown if there is not any valid data. - float temperature(); - - - /// \brief Indicates if anyPressure has valid data. - /// \return true if anyPressure has valid data; otherwise false. - bool hasAnyPressure(); - - /// \brief Gets and converts the latest pressure data regardless of the received - /// underlying type. - /// - /// \return The pressure data. - /// \exception invalid_operation Thrown if there is no valid data. - float anyPressure(); - - /// \brief Indicates if pressure has valid data. - /// \return true if pressure has valid data; otherwise false. - bool hasPressure(); - - /// \brief Pressure data. - /// \return The pressure data. - /// \exception invalid_operation Thrown if there is not any valid data. - float pressure(); - - /// \brief Indicates if anyPosition has valid data. - /// \return true if anyPosition has valid data; otherwise false. - bool hasAnyPosition(); - - /// \brief Gets the latest position data regardless of the received - /// underlying type. - /// - /// \return The position data. - /// \exception invalid_operation Thrown if there is no valid data. - math::PositionD anyPosition(); + /// \brief Indicates if anyPressure has valid data. + /// \return true if anyPressure has valid data; otherwise false. + bool hasAnyPressure(); + + /// \brief Gets and converts the latest pressure data regardless of the received + /// underlying type. + /// + /// \return The pressure data. + /// \exception invalid_operation Thrown if there is no valid data. + float anyPressure(); + + /// \brief Indicates if pressure has valid data. + /// \return true if pressure has valid data; otherwise false. + bool hasPressure(); + + /// \brief Pressure data. + /// \return The pressure data. + /// \exception invalid_operation Thrown if there is not any valid data. + float pressure(); + + /// \brief Indicates if anyPosition has valid data. + /// \return true if anyPosition has valid data; otherwise false. + bool hasAnyPosition(); + + /// \brief Gets the latest position data regardless of the received + /// underlying type. + /// + /// \return The position data. + /// \exception invalid_operation Thrown if there is no valid data. + math::PositionD anyPosition(); /// \brief Indicates if positionGpsLla has valid data. /// \return true if positionGpsLla has valid data; otherwise false. @@ -328,33 +325,33 @@ class vn_proglib_DLLEXPORT CompositeData math::vec3d positionGps2Ecef(); /// \brief Indicates if positionEstimatedLla has valid data. - /// \return true if positionEstimatedLla has valid data; otherwise false. - bool hasPositionEstimatedLla(); - - /// \brief Position Estimated LLA data. - /// \return The Position Estimated LLA data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3d positionEstimatedLla(); - - /// \brief Indicates if positionEstimatedEcef has valid data. - /// \return true if positionEstimatedEcef has valid data; otherwise false. - bool hasPositionEstimatedEcef(); - - /// \brief Position Estimated ECEF data. - /// \return The Position Estimated ECEF data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3d positionEstimatedEcef(); - - /// \brief Indicates if anyVelocity has valid data. - /// \return true if anyVelocity has valid data; otherwise false. - bool hasAnyVelocity(); - - /// \brief Gets the latest velocity data regardless of the received - /// underlying type. - /// - /// \return The velocity data. - /// \exception invalid_operation Thrown if there is no valid data. - math::vec3f anyVelocity(); + /// \return true if positionEstimatedLla has valid data; otherwise false. + bool hasPositionEstimatedLla(); + + /// \brief Position Estimated LLA data. + /// \return The Position Estimated LLA data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3d positionEstimatedLla(); + + /// \brief Indicates if positionEstimatedEcef has valid data. + /// \return true if positionEstimatedEcef has valid data; otherwise false. + bool hasPositionEstimatedEcef(); + + /// \brief Position Estimated ECEF data. + /// \return The Position Estimated ECEF data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3d positionEstimatedEcef(); + + /// \brief Indicates if anyVelocity has valid data. + /// \return true if anyVelocity has valid data; otherwise false. + bool hasAnyVelocity(); + + /// \brief Gets the latest velocity data regardless of the received + /// underlying type. + /// + /// \return The velocity data. + /// \exception invalid_operation Thrown if there is no valid data. + math::vec3f anyVelocity(); /// \brief Indicates if velocityGpsNed has valid data. /// \return true if velocityGpsNed has valid data; otherwise false. @@ -393,67 +390,67 @@ class vn_proglib_DLLEXPORT CompositeData math::vec3f velocityGps2Ecef(); /// \brief Indicates if velocityEstimatedNed has valid data. - /// \return true if velocityEstimatedNed has valid data; otherwise false. - bool hasVelocityEstimatedNed(); - - /// \brief Velocity Estimated NED data. - /// \return The velocity estimated NED data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f velocityEstimatedNed(); - - /// \brief Indicates if velocityEstimatedEcef has valid data. - /// \return true if velocityEstimatedEcef has valid data; otherwise false. - bool hasVelocityEstimatedEcef(); - - /// \brief Velocity Estimated ECEF data. - /// \return The velocity estimated ECEF data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f velocityEstimatedEcef(); - - /// \brief Indicates if velocityEstimatedBody has valid data. - /// \return true if velocityEstimatedBody has valid data; otherwise false. - bool hasVelocityEstimatedBody(); - - /// \brief Velocity Estimated Body data. - /// \return The velocity estimated body data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f velocityEstimatedBody(); - - /// \brief Indicates if deltaTime has valid data. - /// \return true if deltaTime has valid data; otherwise false. - bool hasDeltaTime(); - - /// \brief Delta time data. - /// \return The delta time data. - /// \exception invalid_operation Thrown if there is not any valid data. - float deltaTime(); - - /// \brief Indicates if deltaTheta has valid data. - /// \return true if deltaTheta has valid data; otherwise false. - bool hasDeltaTheta(); - - /// \brief Delta theta data. - /// \return The delta theta data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f deltaTheta(); - - /// \brief Indicates if deltaVelocity has valid data. - /// \return true if deltaVelocity has valid data; otherwise false. - bool hasDeltaVelocity(); - - /// \brief Delta velocity data. - /// \return The delta velocity data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f deltaVelocity(); - - /// \brief Indicates if timeStartup has valid data. - /// \return true if timeStartup has valid data; otherwise false. - bool hasTimeStartup(); - - /// \brief Time startup data. - /// \return The time startup data. - /// \exception invalid_operation Thrown if there is not any valid data. - uint64_t timeStartup(); + /// \return true if velocityEstimatedNed has valid data; otherwise false. + bool hasVelocityEstimatedNed(); + + /// \brief Velocity Estimated NED data. + /// \return The velocity estimated NED data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f velocityEstimatedNed(); + + /// \brief Indicates if velocityEstimatedEcef has valid data. + /// \return true if velocityEstimatedEcef has valid data; otherwise false. + bool hasVelocityEstimatedEcef(); + + /// \brief Velocity Estimated ECEF data. + /// \return The velocity estimated ECEF data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f velocityEstimatedEcef(); + + /// \brief Indicates if velocityEstimatedBody has valid data. + /// \return true if velocityEstimatedBody has valid data; otherwise false. + bool hasVelocityEstimatedBody(); + + /// \brief Velocity Estimated Body data. + /// \return The velocity estimated body data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f velocityEstimatedBody(); + + /// \brief Indicates if deltaTime has valid data. + /// \return true if deltaTime has valid data; otherwise false. + bool hasDeltaTime(); + + /// \brief Delta time data. + /// \return The delta time data. + /// \exception invalid_operation Thrown if there is not any valid data. + float deltaTime(); + + /// \brief Indicates if deltaTheta has valid data. + /// \return true if deltaTheta has valid data; otherwise false. + bool hasDeltaTheta(); + + /// \brief Delta theta data. + /// \return The delta theta data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f deltaTheta(); + + /// \brief Indicates if deltaVelocity has valid data. + /// \return true if deltaVelocity has valid data; otherwise false. + bool hasDeltaVelocity(); + + /// \brief Delta velocity data. + /// \return The delta velocity data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f deltaVelocity(); + + /// \brief Indicates if timeStartup has valid data. + /// \return true if timeStartup has valid data; otherwise false. + bool hasTimeStartup(); + + /// \brief Time startup data. + /// \return The time startup data. + /// \exception invalid_operation Thrown if there is not any valid data. + uint64_t timeStartup(); /// \brief Indicates if timeGps has valid data. /// \return true if timeGps has valid data; otherwise false. @@ -483,58 +480,58 @@ class vn_proglib_DLLEXPORT CompositeData double tow(); /// \brief Indicates if week has valid data. - /// \return true if week has valid data; otherwise false. - bool hasWeek(); - - /// \brief Week data. - /// \return The week data. - /// \exception invalid_operation Thrown if there is not any valid data. - uint16_t week(); - - /// \brief Indicates if numSats has valid data. - /// \return true if numSats has valid data; otherwise false. - bool hasNumSats(); - - /// \brief NumSats data. - /// \return The numsats data. - /// \exception invalid_operation Thrown if there is not any valid data. - uint8_t numSats(); - - /// \brief Indicates if timeSyncIn has valid data. - /// \return true if timeSyncIn has valid data; otherwise false. - bool hasTimeSyncIn(); - - /// \brief TimeSyncIn data. - /// \return The TimeSyncIn data. - /// \exception invalid_operation Thrown if there is not any valid data. - uint64_t timeSyncIn(); - - /// \brief Indicates if vpeStatus has valid data. - /// \return true if vpeStatus has valid data; otherwise false. - bool hasVpeStatus(); - - /// \brief VpeStatus data. - /// \return The VpeStatus data. - /// \exception invalid_operation Thrown if there is not any valid data. - protocol::uart::VpeStatus vpeStatus(); - - /// \brief Indicates if insStatus has valid data. - /// \return true if insStatus has valid data; otherwise false. - bool hasInsStatus(); - - /// \brief InsStatus data. - /// \return The InsStatus data. - /// \exception invalid_operation Thrown if there is not any valid data. - protocol::uart::InsStatus insStatus(); - - /// \brief Indicates if syncInCnt has valid data. - /// \return true if syncInCnt has valid data; otherwise false. - bool hasSyncInCnt(); - - /// \brief SyncInCnt data. - /// \return The SyncInCnt data. - /// \exception invalid_operation Thrown if there is not any valid data. - uint32_t syncInCnt(); + /// \return true if week has valid data; otherwise false. + bool hasWeek(); + + /// \brief Week data. + /// \return The week data. + /// \exception invalid_operation Thrown if there is not any valid data. + uint16_t week(); + + /// \brief Indicates if numSats has valid data. + /// \return true if numSats has valid data; otherwise false. + bool hasNumSats(); + + /// \brief NumSats data. + /// \return The numsats data. + /// \exception invalid_operation Thrown if there is not any valid data. + uint8_t numSats(); + + /// \brief Indicates if timeSyncIn has valid data. + /// \return true if timeSyncIn has valid data; otherwise false. + bool hasTimeSyncIn(); + + /// \brief TimeSyncIn data. + /// \return The TimeSyncIn data. + /// \exception invalid_operation Thrown if there is not any valid data. + uint64_t timeSyncIn(); + + /// \brief Indicates if vpeStatus has valid data. + /// \return true if vpeStatus has valid data; otherwise false. + bool hasVpeStatus(); + + /// \brief VpeStatus data. + /// \return The VpeStatus data. + /// \exception invalid_operation Thrown if there is not any valid data. + protocol::uart::VpeStatus vpeStatus(); + + /// \brief Indicates if insStatus has valid data. + /// \return true if insStatus has valid data; otherwise false. + bool hasInsStatus(); + + /// \brief InsStatus data. + /// \return The InsStatus data. + /// \exception invalid_operation Thrown if there is not any valid data. + protocol::uart::InsStatus insStatus(); + + /// \brief Indicates if syncInCnt has valid data. + /// \return true if syncInCnt has valid data; otherwise false. + bool hasSyncInCnt(); + + /// \brief SyncInCnt data. + /// \return The SyncInCnt data. + /// \exception invalid_operation Thrown if there is not any valid data. + uint32_t syncInCnt(); /// \brief Indicates if syncOutCnt has valid data. /// \return true if syncOutCnt has valid data; otherwise false. @@ -591,22 +588,22 @@ class vn_proglib_DLLEXPORT CompositeData uint64_t gps2Tow(); /// \brief Indicates if timeUtc has valid data. - /// \return true if timeUtc has valid data; otherwise false. - bool hasTimeUtc(); + /// \return true if timeUtc has valid data; otherwise false. + bool hasTimeUtc(); - /// \brief TimeUtc data. - /// \return The TimeUtc data. - /// \exception invalid_operation Thrown if there is not any valid data. - protocol::uart::TimeUtc timeUtc(); + /// \brief TimeUtc data. + /// \return The TimeUtc data. + /// \exception invalid_operation Thrown if there is not any valid data. + protocol::uart::TimeUtc timeUtc(); - /// \brief Indicates if sensSat has valid data. - /// \return true if sensSat has valid data; otherwise false. - bool hasSensSat(); + /// \brief Indicates if sensSat has valid data. + /// \return true if sensSat has valid data; otherwise false. + bool hasSensSat(); - /// \brief SensSat data. - /// \return The SensSat data. - /// \exception invalid_operation Thrown if there is not any valid data. - protocol::uart::SensSat sensSat(); + /// \brief SensSat data. + /// \return The SensSat data. + /// \exception invalid_operation Thrown if there is not any valid data. + protocol::uart::SensSat sensSat(); /// \brief Indicates if fix has valid data. /// \return true if fix has valid data; otherwise false. @@ -627,15 +624,15 @@ class vn_proglib_DLLEXPORT CompositeData protocol::uart::GpsFix fix2(); /// \brief Indicates if anyPositionUncertainty has valid data. - /// \return true if anyPositionUncertainty has valid data; otherwise false. - bool hasAnyPositionUncertainty(); + /// \return true if anyPositionUncertainty has valid data; otherwise false. + bool hasAnyPositionUncertainty(); - /// \brief Gets the latest position uncertainty data regardless of the received - /// underlying type. - /// - /// \return The position uncertainty data. - /// \exception invalid_operation Thrown if there is no valid data. - math::vec3f anyPositionUncertainty(); + /// \brief Gets the latest position uncertainty data regardless of the received + /// underlying type. + /// + /// \return The position uncertainty data. + /// \exception invalid_operation Thrown if there is no valid data. + math::vec3f anyPositionUncertainty(); /// \brief Indicates if positionUncertaintyGpsNed has valid data. /// \return true if positionUncertaintyGpsNed has valid data; otherwise false. @@ -646,9 +643,9 @@ class vn_proglib_DLLEXPORT CompositeData bool hasPositionUncertaintyGps2Ned(); /// \brief GPS position uncertainty NED data. - /// \return The GPS position uncertainty NED data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f positionUncertaintyGpsNed(); + /// \return The GPS position uncertainty NED data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f positionUncertaintyGpsNed(); /// \brief GPS2 position uncertainty NED data. /// \return The GPS2 position uncertainty NED data. @@ -664,9 +661,9 @@ class vn_proglib_DLLEXPORT CompositeData bool hasPositionUncertaintyGps2Ecef(); /// \brief GPS position uncertainty ECEF data. - /// \return The GPS position uncertainty ECEF data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f positionUncertaintyGpsEcef(); + /// \return The GPS position uncertainty ECEF data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f positionUncertaintyGpsEcef(); /// \brief GPS2 position uncertainty ECEF data. /// \return The GPS2 position uncertainty ECEF data. @@ -674,24 +671,24 @@ class vn_proglib_DLLEXPORT CompositeData math::vec3f positionUncertaintyGps2Ecef(); /// \brief Indicates if positionUncertaintyEstimated has valid data. - /// \return true if positionUncertaintyEstimated has valid data; otherwise false. - bool hasPositionUncertaintyEstimated(); + /// \return true if positionUncertaintyEstimated has valid data; otherwise false. + bool hasPositionUncertaintyEstimated(); - /// \brief Estimated position uncertainty data. - /// \return The estimated position uncertainty data. - /// \exception invalid_operation Thrown if there is not any valid data. - float positionUncertaintyEstimated(); + /// \brief Estimated position uncertainty data. + /// \return The estimated position uncertainty data. + /// \exception invalid_operation Thrown if there is not any valid data. + float positionUncertaintyEstimated(); - /// \brief Indicates if anyVelocityUncertainty has valid data. - /// \return true if anyVelocityUncertainty has valid data; otherwise false. - bool hasAnyVelocityUncertainty(); + /// \brief Indicates if anyVelocityUncertainty has valid data. + /// \return true if anyVelocityUncertainty has valid data; otherwise false. + bool hasAnyVelocityUncertainty(); - /// \brief Gets the latest velocity uncertainty data regardless of the received - /// underlying type. - /// - /// \return The velocity uncertainty data. - /// \exception invalid_operation Thrown if there is no valid data. - float anyVelocityUncertainty(); + /// \brief Gets the latest velocity uncertainty data regardless of the received + /// underlying type. + /// + /// \return The velocity uncertainty data. + /// \exception invalid_operation Thrown if there is no valid data. + float anyVelocityUncertainty(); /// \brief Indicates if velocityUncertaintyGps has valid data. /// \return true if velocityUncertaintyGps has valid data; otherwise false. @@ -712,51 +709,51 @@ class vn_proglib_DLLEXPORT CompositeData float velocityUncertaintyGps2(); /// \brief Indicates if velocityUncertaintyEstimated has valid data. - /// \return true if velocityUncertaintyEstimated has valid data; otherwise false. - bool hasVelocityUncertaintyEstimated(); - - /// \brief Estimated velocity uncertainty data. - /// \return The estimated velocity uncertainty data. - /// \exception invalid_operation Thrown if there is not any valid data. - float velocityUncertaintyEstimated(); - - /// \brief Indicates if timeUncertainty has valid data. - /// \return true if timeUncertainty has valid data; otherwise false. - bool hasTimeUncertainty(); - - /// \brief Time uncertainty data. - /// \return The time uncertainty data. - /// \exception invalid_operation Thrown if there is not any valid data. - uint32_t timeUncertainty(); - - /// \brief Indicates if attitudeUncertainty has valid data. - /// \return true if attitudeUncertainty has valid data; otherwise false. - bool hasAttitudeUncertainty(); - - /// \brief Attitude uncertainty data. - /// \return The attitude uncertainty data. - /// \exception invalid_operation Thrown if there is not any valid data. - math::vec3f attitudeUncertainty(); - - /// \brief Indicates if courseOverGround has valid data. - /// \return true if courseOverGround havs valid data; otherwise false. - bool hasCourseOverGround(); - - /// \brief Computes the course over ground from any velocity data available. - /// - /// \return The computed course over ground. - /// \exception invalid_operation Thrown if there is no valid data. - float courseOverGround(); - - /// \brief Indicates if speedOverGround has valid data. - /// \return true if speedOverGround havs valid data; otherwise false. - bool hasSpeedOverGround(); - - /// \brief Computes the speed over ground from any velocity data available. - /// - /// \return The computed speed over ground. - /// \exception invalid_operation Thrown if there is no valid data. - float speedOverGround(); + /// \return true if velocityUncertaintyEstimated has valid data; otherwise false. + bool hasVelocityUncertaintyEstimated(); + + /// \brief Estimated velocity uncertainty data. + /// \return The estimated velocity uncertainty data. + /// \exception invalid_operation Thrown if there is not any valid data. + float velocityUncertaintyEstimated(); + + /// \brief Indicates if timeUncertainty has valid data. + /// \return true if timeUncertainty has valid data; otherwise false. + bool hasTimeUncertainty(); + + /// \brief Time uncertainty data. + /// \return The time uncertainty data. + /// \exception invalid_operation Thrown if there is not any valid data. + uint32_t timeUncertainty(); + + /// \brief Indicates if attitudeUncertainty has valid data. + /// \return true if attitudeUncertainty has valid data; otherwise false. + bool hasAttitudeUncertainty(); + + /// \brief Attitude uncertainty data. + /// \return The attitude uncertainty data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f attitudeUncertainty(); + + /// \brief Indicates if courseOverGround has valid data. + /// \return true if courseOverGround havs valid data; otherwise false. + bool hasCourseOverGround(); + + /// \brief Computes the course over ground from any velocity data available. + /// + /// \return The computed course over ground. + /// \exception invalid_operation Thrown if there is no valid data. + float courseOverGround(); + + /// \brief Indicates if speedOverGround has valid data. + /// \return true if speedOverGround havs valid data; otherwise false. + bool hasSpeedOverGround(); + + /// \brief Computes the speed over ground from any velocity data available. + /// + /// \return The computed speed over ground. + /// \exception invalid_operation Thrown if there is no valid data. + float speedOverGround(); /// \brief Indicates if timeInfo has valid data. /// \return true if timeInfo havs valid data; otherwise false. @@ -784,33 +781,37 @@ class vn_proglib_DLLEXPORT CompositeData /// \exception invalid_operation Thrown if there is no valid data. protocol::uart::GnssDop dop(); - private: - static void parseBinary(protocol::uart::Packet& p, std::vector& o); - static void parseAscii(protocol::uart::Packet& p, std::vector& o); - static void parseBinaryPacketCommonGroup(protocol::uart::Packet& p, protocol::uart::CommonGroup gf, std::vector& o); - static void parseBinaryPacketTimeGroup(protocol::uart::Packet& p, protocol::uart::TimeGroup gf, std::vector& o); - static void parseBinaryPacketImuGroup(protocol::uart::Packet& p, protocol::uart::ImuGroup gf, std::vector& o); - static void parseBinaryPacketGpsGroup(protocol::uart::Packet& p, protocol::uart::GpsGroup gf, std::vector& o); - static void parseBinaryPacketAttitudeGroup(protocol::uart::Packet& p, protocol::uart::AttitudeGroup gf, std::vector& o); - static void parseBinaryPacketInsGroup(protocol::uart::Packet& p, protocol::uart::InsGroup gf, std::vector& o); - static void parseBinaryPacketGps2Group(protocol::uart::Packet& p, protocol::uart::GpsGroup gf, std::vector& o); + static void parseBinary(protocol::uart::Packet & p, std::vector & o); + static void parseAscii(protocol::uart::Packet & p, std::vector & o); + static void parseBinaryPacketCommonGroup( + protocol::uart::Packet & p, protocol::uart::CommonGroup gf, std::vector & o); + static void parseBinaryPacketTimeGroup( + protocol::uart::Packet & p, protocol::uart::TimeGroup gf, std::vector & o); + static void parseBinaryPacketImuGroup( + protocol::uart::Packet & p, protocol::uart::ImuGroup gf, std::vector & o); + static void parseBinaryPacketGpsGroup( + protocol::uart::Packet & p, protocol::uart::GpsGroup gf, std::vector & o); + static void parseBinaryPacketAttitudeGroup( + protocol::uart::Packet & p, protocol::uart::AttitudeGroup gf, std::vector & o); + static void parseBinaryPacketInsGroup( + protocol::uart::Packet & p, protocol::uart::InsGroup gf, std::vector & o); + static void parseBinaryPacketGps2Group( + protocol::uart::Packet & p, protocol::uart::GpsGroup gf, std::vector & o); private: - struct Impl; - Impl* _i; - - template - static void setValues(T val, std::vector& o, void (Impl::* function)(T)) - { - for (std::vector::iterator i = o.begin(); i != o.end(); ++i) - (*((*i)->_i).*function)(val); - } + struct Impl; + Impl * _i; + + template + static void setValues(T val, std::vector & o, void (Impl::*function)(T)) + { + for (std::vector::iterator i = o.begin(); i != o.end(); ++i) + (*((*i)->_i).*function)(val); + } }; - - -} -} +} // namespace sensors +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/consts.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/consts.h index 0d087633..0c67e42f 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/consts.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/consts.h @@ -6,8 +6,10 @@ #ifndef _VN_MATH_CONSTS_H_ #define _VN_MATH_CONSTS_H_ -namespace vn { -namespace math { +namespace vn +{ +namespace math +{ /// \defgroup pi_constants PI Constants /// \brief Convenient PI constants. @@ -42,7 +44,7 @@ static const double PIHd = 1.570796326794896619; /// \} -} -} +} // namespace math +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/conversions.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/conversions.h index 97e85ea3..3683adfb 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/conversions.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/conversions.h @@ -2,11 +2,13 @@ #define _VN_MATH_CONVERSIONS_H_ #include "export.h" -#include "vector.h" #include "matrix.h" +#include "vector.h" -namespace vn { -namespace math { +namespace vn +{ +namespace math +{ /// \defgroup angle_convertors Angle Convertors /// \brief Methods useful for angle conversions. @@ -28,13 +30,12 @@ double rad2deg(double angleInRads); /// /// \param[in] anglesInRads The vector of angles in radians. /// \return The converted angles. -template +template vec rad2deg(vec anglesInRads) { - for (size_t i = 0; i < dim; i++) - anglesInRads[i] = rad2deg(anglesInRads[i]); + for (size_t i = 0; i < dim; i++) anglesInRads[i] = rad2deg(anglesInRads[i]); - return anglesInRads; + return anglesInRads; } /// \brief Converts an angle in degrees to radians. @@ -55,13 +56,12 @@ double vn_proglib_DLLEXPORT deg2rad(double angleInDegs); /// \return The converted angles. //template //vec vn_proglib_DLLEXPORT deg2rad(vec anglesInDegs) -template +template vec deg2rad(vec anglesInDegs) { - for (size_t i = 0; i < dim; i++) - anglesInDegs[i] = deg2rad(anglesInDegs[i]); + for (size_t i = 0; i < dim; i++) anglesInDegs[i] = deg2rad(anglesInDegs[i]); - return anglesInDegs; + return anglesInDegs; } /// \} @@ -264,7 +264,7 @@ vec3f vn_proglib_DLLEXPORT yprInDegs2omegaPhiKappaInRads(vec3f yprDegs); /// \return The omega, phi, kappa representation radians. vec3f vn_proglib_DLLEXPORT yprInRads2omegaPhiKappaInRads(vec3f yprRads); -} -} +} // namespace math +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/criticalsection.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/criticalsection.h index fdee2566..3ad0471f 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/criticalsection.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/criticalsection.h @@ -6,47 +6,44 @@ #ifndef _VNXPLAT_CRITICALSECTION_H_ #define _VNXPLAT_CRITICALSECTION_H_ -#include "nocopy.h" #include "export.h" +#include "nocopy.h" -namespace vn { -namespace xplat { +namespace vn +{ +namespace xplat +{ /// \brief Represents a cross-platform critical section. class vn_proglib_DLLEXPORT CriticalSection : private util::NoCopy { - - // Constructors /////////////////////////////////////////////////////////// + // Constructors /////////////////////////////////////////////////////////// public: + /// \brief Creates a new critical section. + CriticalSection(); - /// \brief Creates a new critical section. - CriticalSection(); + ~CriticalSection(); - ~CriticalSection(); - - // Public Methods ///////////////////////////////////////////////////////// + // Public Methods ///////////////////////////////////////////////////////// public: + /// \brief Requests and signals that a critical section is being entered. + void enter(); - /// \brief Requests and signals that a critical section is being entered. - void enter(); - - /// \brief Signals that a critical section is being left. - void leave(); + /// \brief Signals that a critical section is being left. + void leave(); - // Private Members //////////////////////////////////////////////////////// + // Private Members //////////////////////////////////////////////////////// private: - - // Contains internal data, mainly stuff that is required for cross-platform - // support. - struct Impl; - Impl *_pi; - + // Contains internal data, mainly stuff that is required for cross-platform + // support. + struct Impl; + Impl * _pi; }; -} -} +} // namespace xplat +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/dllvalidator.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/dllvalidator.h index 32271258..a62732d8 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/dllvalidator.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/dllvalidator.h @@ -7,17 +7,17 @@ #include #if defined(_MSC_VER) - // Disable a bunch of warnings from 3rd party library. - #pragma warning(push) - #pragma warning(disable:4091) - #pragma warning(disable:4251) - #pragma warning(disable:4275) +// Disable a bunch of warnings from 3rd party library. +#pragma warning(push) +#pragma warning(disable : 4091) +#pragma warning(disable : 4251) +#pragma warning(disable : 4275) #endif #include "PeLib.h" -#if defined (_MSC_VER) - #pragma warning(pop) +#if defined(_MSC_VER) +#pragma warning(pop) #endif // Class to validate the input DLL exists and that all of it's first level @@ -25,40 +25,40 @@ class DllValidator { public: - DllValidator(std::string dllName, std::string currentDirectory); + DllValidator(std::string dllName, std::string currentDirectory); - bool initialize(); + bool initialize(); - bool hasDllNames(); + bool hasDllNames(); - void getDllNames(std::vector& dllNamesOut); + void getDllNames(std::vector & dllNamesOut); - void getMissingDllNames(std::vector& missingDllNamesOut); + void getMissingDllNames(std::vector & missingDllNamesOut); - bool validate(); + bool validate(); private: - struct DllValidatorVisitor : public PeLib::PeFileVisitor - { - //template - //void dumpImportDirectory(PeLib::PeFile& pef, std::vector& dllNamesOut); + struct DllValidatorVisitor : public PeLib::PeFileVisitor + { + //template + //void dumpImportDirectory(PeLib::PeFile& pef, std::vector& dllNamesOut); - virtual void callback(PeLib::PeFile32 &file); + virtual void callback(PeLib::PeFile32 & file); - virtual void callback(PeLib::PeFile64 &file); + virtual void callback(PeLib::PeFile64 & file); - std::vector mRequiredDlls; - }; + std::vector mRequiredDlls; + }; - DllValidator(); + DllValidator(); - bool mIsInitialized; - bool mIsValid; - DllValidatorVisitor mVisitor; - PeLib::PeFile* mPeFile; - std::string mFileName; - std::string mWorkingDirectory; - std::vector mMissingDlls; + bool mIsInitialized; + bool mIsValid; + DllValidatorVisitor mVisitor; + PeLib::PeFile * mPeFile; + std::string mFileName; + std::string mWorkingDirectory; + std::vector mMissingDlls; }; #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/error_detection.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/error_detection.h index 65c77e55..95428cdc 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/error_detection.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/error_detection.h @@ -6,48 +6,45 @@ #include "int.h" -namespace vn { -namespace data { -namespace integrity { +namespace vn +{ +namespace data +{ +namespace integrity +{ /// \brief Helpful class for working with 8-bit checksums. class Checksum8 { - - // Public Methods ///////////////////////////////////////////////////////// + // Public Methods ///////////////////////////////////////////////////////// public: - - /// \brief Computes the 8-bit checksum of the provided data. - /// - /// \param[in] data The data array to compute the 8-bit checksum for. - /// \param[in] length The length of data bytes from the array to compute - /// the checksum over. - /// \return The computed checksum. - static uint8_t compute(const char data[], size_t length); - + /// \brief Computes the 8-bit checksum of the provided data. + /// + /// \param[in] data The data array to compute the 8-bit checksum for. + /// \param[in] length The length of data bytes from the array to compute + /// the checksum over. + /// \return The computed checksum. + static uint8_t compute(const char data[], size_t length); }; /// \brief Helpful class for working with 16-bit CRCs. class Crc16 { - - // Public Methods ///////////////////////////////////////////////////////// + // Public Methods ///////////////////////////////////////////////////////// public: - - /// \brief Computes the 16-bit CRC of the provided data. - /// - /// \param[in] data The data array to compute the 16-bit CRC for. - /// \param[in] length The length of data bytes from the array to compute - /// the CRC over. - /// \return The computed CRC. - static uint16_t compute(const char data[], size_t length); - + /// \brief Computes the 16-bit CRC of the provided data. + /// + /// \param[in] data The data array to compute the 16-bit CRC for. + /// \param[in] length The length of data bytes from the array to compute + /// the CRC over. + /// \return The computed CRC. + static uint16_t compute(const char data[], size_t length); }; -} -} -} +} // namespace integrity +} // namespace data +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/event.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/event.h index 591a4f67..40f3cff1 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/event.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/event.h @@ -1,73 +1,68 @@ #ifndef _VNXPLAT_EVENT_H_ #define _VNXPLAT_EVENT_H_ -#include "nocopy.h" -#include "int.h" #include "export.h" +#include "int.h" +#include "nocopy.h" -namespace vn { -namespace xplat { +namespace vn +{ +namespace xplat +{ /// \brief Represents a cross-platform event. class vn_proglib_DLLEXPORT Event : private util::NoCopy { - public: + /// \brief Available wait results. + enum WaitResult { + WAIT_SIGNALED, ///< The event was signalled. + WAIT_TIMEDOUT ///< Timed out while waiting for the event to signal. + }; - /// \brief Available wait results. - enum WaitResult - { - WAIT_SIGNALED, ///< The event was signalled. - WAIT_TIMEDOUT ///< Timed out while waiting for the event to signal. - }; - - // Constructors /////////////////////////////////////////////////////////// + // Constructors /////////////////////////////////////////////////////////// public: + /// \brief Creates a new event. + Event(); - /// \brief Creates a new event. - Event(); + ~Event(); - ~Event(); - - // Public Methods ///////////////////////////////////////////////////////// + // Public Methods ///////////////////////////////////////////////////////// public: - - /// \brief Waits for a signal on this event. - /// - /// This method will wait indefinitely for the event. - void wait(); - - /// \brief Waits for a signal on this event for the specified amount of - /// time. - /// - /// \param[in] timeoutUs The amount of time to wait in microseconds. - /// \return The result of the wait operation. - WaitResult waitUs(uint32_t timeoutUs); - - /// \brief Waits for a signal on this event for the specified amount of - /// time. - /// - /// \param[in] timeoutMs The amount of time to wait in milliseconds. - /// \return The result of the wait operation. - WaitResult waitMs(uint32_t timeoutMs); - - /// \brief Signals the event. - void signal(); - - // Private Members //////////////////////////////////////////////////////// + /// \brief Waits for a signal on this event. + /// + /// This method will wait indefinitely for the event. + void wait(); + + /// \brief Waits for a signal on this event for the specified amount of + /// time. + /// + /// \param[in] timeoutUs The amount of time to wait in microseconds. + /// \return The result of the wait operation. + WaitResult waitUs(uint32_t timeoutUs); + + /// \brief Waits for a signal on this event for the specified amount of + /// time. + /// + /// \param[in] timeoutMs The amount of time to wait in milliseconds. + /// \return The result of the wait operation. + WaitResult waitMs(uint32_t timeoutMs); + + /// \brief Signals the event. + void signal(); + + // Private Members //////////////////////////////////////////////////////// private: - - // Contains internal data, mainly stuff that is required for cross-platform - // support. - struct Impl; - Impl *_pi; - + // Contains internal data, mainly stuff that is required for cross-platform + // support. + struct Impl; + Impl * _pi; }; -} -} +} // namespace xplat +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/exceptions.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/exceptions.h index ce8faa4e..26a2f5eb 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/exceptions.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/exceptions.h @@ -9,20 +9,21 @@ #include #include -namespace vn { +namespace vn +{ /// \brief Exception class indicating that there as an dimensional error. class dimension_error : public std::runtime_error { public: - dimension_error() : runtime_error("dimension") { } + dimension_error() : runtime_error("dimension") {} }; /// \brief Indicates an unknown error occurred. class unknown_error : public std::exception { public: - unknown_error() : exception() { } + unknown_error() : exception() {} }; /// \brief Indicates that the requested functionality is not currently @@ -30,70 +31,73 @@ class unknown_error : public std::exception class not_implemented : public std::logic_error { public: - not_implemented() : logic_error("Not implemented.") { } - explicit not_implemented(std::string msg) : logic_error(msg.c_str()) { } + not_implemented() : logic_error("Not implemented.") {} + explicit not_implemented(std::string msg) : logic_error(msg.c_str()) {} }; /// \brief Indicates a null pointer was provided. class null_pointer : public std::exception { public: - null_pointer() : exception() { } + null_pointer() : exception() {} }; /// \brief Indicates an invalid operation was attempted. class invalid_operation : public std::runtime_error { public: - invalid_operation() : runtime_error("invalid operation") { } - explicit invalid_operation(std::string msg) : runtime_error(msg.c_str()) { } + invalid_operation() : runtime_error("invalid operation") {} + explicit invalid_operation(std::string msg) : runtime_error(msg.c_str()) {} }; /// \brief Indicates invalid permission for the operation. class permission_denied : public std::runtime_error { public: - permission_denied() : runtime_error("permission denied") { } - #if VN_SUPPORTS_CSTR_STRING_CONCATENATE - explicit permission_denied(std::string itemName) : runtime_error(std::string("Permission denied for item '" + itemName + "'.").c_str()) { } - #else - explicit permission_denied(std::string itemName) : runtime_error("Permission denied for item.") { } - #endif + permission_denied() : runtime_error("permission denied") {} +#if VN_SUPPORTS_CSTR_STRING_CONCATENATE + explicit permission_denied(std::string itemName) + : runtime_error(std::string("Permission denied for item '" + itemName + "'.").c_str()) + { + } +#else + explicit permission_denied(std::string itemName) : runtime_error("Permission denied for item.") {} +#endif }; /// \brief Indicates the requested feature is not supported. class not_supported : public std::exception { public: - not_supported() : exception() { } + not_supported() : exception() {} }; /// \brief Requested item not found. class not_found : public std::runtime_error { public: - explicit not_found(std::string msg) : runtime_error(msg) { } + explicit not_found(std::string msg) : runtime_error(msg) {} }; /// \brief The format was invalid. class invalid_format : public std::exception { public: - invalid_format() : exception() { } + invalid_format() : exception() {} }; /// \brief A timeout occurred. class timeout : public std::exception { public: - timeout() : exception() { } + timeout() : exception() {} - /// \brief Returns a description of the exception. - /// - /// \return A description of the exception. - char const* what() const throw() { return "timeout"; } + /// \brief Returns a description of the exception. + /// + /// \return A description of the exception. + char const * what() const throw() { return "timeout"; } }; -} +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/export.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/export.h index b6e130d8..4c20e04c 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/export.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/export.h @@ -2,23 +2,23 @@ #define _VN_UTIL_EXPORT_H #if defined _WINDOWS && defined _BUILD_DLL - #if defined proglib_cpp_EXPORTS - #define vn_proglib_DLLEXPORT __declspec(dllexport) - #else - #define vn_proglib_DLLEXPORT __declspec(dllimport) - #endif +#if defined proglib_cpp_EXPORTS +#define vn_proglib_DLLEXPORT __declspec(dllexport) #else - #define vn_proglib_DLLEXPORT +#define vn_proglib_DLLEXPORT __declspec(dllimport) +#endif +#else +#define vn_proglib_DLLEXPORT #endif #if defined _WINDOWS && defined _BUILD_DLL - #if defined proglib_cpp_graphics_EXPORTS - #define vn_proglib_graphics_DLLEXPORT __declspec(dllexport) - #else - #define vn_proglib_graphics_DLLEXPORT __declspec(dllimport) - #endif +#if defined proglib_cpp_graphics_EXPORTS +#define vn_proglib_graphics_DLLEXPORT __declspec(dllexport) +#else +#define vn_proglib_graphics_DLLEXPORT __declspec(dllimport) +#endif #else - #define vn_proglib_graphics_DLLEXPORT +#define vn_proglib_graphics_DLLEXPORT #endif #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/ezasyncdata.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/ezasyncdata.h index 28462091..f0952b80 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/ezasyncdata.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/ezasyncdata.h @@ -3,74 +3,76 @@ #include -#include "vn/int.h" -#include "vn/nocopy.h" -#include "vn/sensors.h" #include "vn/compositedata.h" #include "vn/criticalsection.h" #include "vn/event.h" #include "vn/export.h" +#include "vn/int.h" +#include "vn/nocopy.h" +#include "vn/sensors.h" -namespace vn { -namespace sensors { +namespace vn +{ +namespace sensors +{ /// \brief Provides easy and reliable access to asynchronous data from a /// VectorNav sensor at the cost of a slight performance hit. class vn_proglib_DLLEXPORT EzAsyncData : private util::NoCopy { private: - explicit EzAsyncData(VnSensor* sensor); + explicit EzAsyncData(VnSensor * sensor); public: - /// \brief DTOR - ~EzAsyncData(); + /// \brief DTOR + ~EzAsyncData(); - /// \brief Returns the underlying sensor object. - /// \return The sensor object. - VnSensor* sensor(); + /// \brief Returns the underlying sensor object. + /// \return The sensor object. + VnSensor * sensor(); - /// \brief Connects to a sensor with the specified connection parameters. - /// - /// \param[in] portName The COM port name. - /// \param[in] baudrate Baudrate to connect to the sensor at. - /// \return New EzAsyncData object wrapping the connected sensor and - /// providing easy access to asynchronous data. - static EzAsyncData* connect(std::string portName, uint32_t baudrate); + /// \brief Connects to a sensor with the specified connection parameters. + /// + /// \param[in] portName The COM port name. + /// \param[in] baudrate Baudrate to connect to the sensor at. + /// \return New EzAsyncData object wrapping the connected sensor and + /// providing easy access to asynchronous data. + static EzAsyncData * connect(std::string portName, uint32_t baudrate); - /// \brief Disconnects from the VectorNav sensor. - void disconnect(); + /// \brief Disconnects from the VectorNav sensor. + void disconnect(); - /// \brief Gets the latest collection of current data received from asychronous - /// messages from the sensor. - /// - /// \return The latest current data. - CompositeData currentData(); + /// \brief Gets the latest collection of current data received from asychronous + /// messages from the sensor. + /// + /// \return The latest current data. + CompositeData currentData(); - /// \brief Gets the next data packet available and blocks until a data - /// packet is received if there is currently not data available. - /// - /// \return The received data packet. - CompositeData getNextData(); + /// \brief Gets the next data packet available and blocks until a data + /// packet is received if there is currently not data available. + /// + /// \return The received data packet. + CompositeData getNextData(); - /// \brief This method will get the next data packet available and block until - /// a data packet is received if there is currently not data available. - /// - /// \param[in] timeoutMs The number of milliseconds to wait for the next available data. - /// \return The next received packet of data. - /// \exception timeout Did not receive new data by the timeout. - CompositeData getNextData(int timeoutMs); + /// \brief This method will get the next data packet available and block until + /// a data packet is received if there is currently not data available. + /// + /// \param[in] timeoutMs The number of milliseconds to wait for the next available data. + /// \return The next received packet of data. + /// \exception timeout Did not receive new data by the timeout. + CompositeData getNextData(int timeoutMs); private: - static void asyncPacketReceivedHandler(void* userData, protocol::uart::Packet& p, size_t index); + static void asyncPacketReceivedHandler(void * userData, protocol::uart::Packet & p, size_t index); private: - VnSensor* _sensor; - xplat::CriticalSection _mainCS, _copyCS; - CompositeData _persistentData, _nextData; - xplat::Event _newDataEvent; + VnSensor * _sensor; + xplat::CriticalSection _mainCS, _copyCS; + CompositeData _persistentData, _nextData; + xplat::Event _newDataEvent; }; -} -} +} // namespace sensors +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/int.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/int.h index 90ca2224..51d9e8e7 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/int.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/int.h @@ -9,20 +9,20 @@ #define _VN_INT_H_ #if defined(_MSC_VER) && _MSC_VER <= 1500 - - // Visual Studio 2008 and earlier do not include the stdint.h header file. - - 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; + +// Visual Studio 2008 and earlier do not include the stdint.h header file. + +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; #else - #include +#include #endif #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/matrix.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/matrix.h index 6efe6ed5..080fc43e 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/matrix.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/matrix.h @@ -9,667 +9,588 @@ #include #include -#include "vector.h" #include "exceptions.h" +#include "vector.h" -namespace vn { -namespace math { +namespace vn +{ +namespace math +{ /// \brief Template for a matrix. template struct mat { - // Public Members ///////////////////////////////////////////////////////// + // Public Members ///////////////////////////////////////////////////////// public: + /// \brief The matrix's elements. + union { + T e[m * n]; + }; - /// \brief The matrix's elements. - union - { - T e[m * n]; - }; - - // Constructors /////////////////////////////////////////////////////////// + // Constructors /////////////////////////////////////////////////////////// public: + /// \brief Creates a new matrix with uninitialized elements. + mat() {} - /// \brief Creates a new matrix with uninitialized elements. - mat() { } - - /// \brief Creates a new matrix with ints elements initialized to val. - /// - /// \param[in] val The initialization value. - explicit mat(T val) - { - std::fill_n(e, m * n, val); - } + /// \brief Creates a new matrix with ints elements initialized to val. + /// + /// \param[in] val The initialization value. + explicit mat(T val) { std::fill_n(e, m * n, val); } - // Helper Methods ///////////////////////////////////////////////////////// + // Helper Methods ///////////////////////////////////////////////////////// public: + /// \brief Matrix with all of its elements set to 0. + /// + /// \return The 0 matrix. + static mat zero() { return mat(0); } - /// \brief Matrix with all of its elements set to 0. - /// - /// \return The 0 matrix. - static mat zero() - { - return mat(0); - } + /// \brief Matrix with all of its elements set to 1. + /// + /// \return The 1 matrix. + static mat one() { return mat(1); } - /// \brief Matrix with all of its elements set to 1. - /// - /// \return The 1 matrix. - static mat one() - { - return mat(1); - } + /// \brief Identity matrix with its diagonal elements set to 1. + /// + /// \return The identity matrix. + static mat identity() + { + assert(m == n); - /// \brief Identity matrix with its diagonal elements set to 1. - /// - /// \return The identity matrix. - static mat identity() - { - assert(m == n); + mat nm(0); - mat nm(0); + for (size_t i = 0; i < m; i++) nm.e[i * m + i] = 1; - for (size_t i = 0; i < m; i++) - nm.e[i * m + i] = 1; + return nm; + } - return nm; - } - - // Operator Overloads ///////////////////////////////////////////////////// + // Operator Overloads ///////////////////////////////////////////////////// public: - - /// \brief Indexing into the matrix's elements. - /// - /// \param[in] row The 0-based index row. - /// \param[in] col The 0-based index column. - /// \return The requested element. - T& operator()(size_t row, size_t col) - { - return const_cast(static_cast(*this)(row, col)); - } - - /// \brief Indexing into the matrix's elements. - /// - /// \param[in] row The 0-based index row. - /// \param[in] col The 0-based index column. - /// \return The requested element. - const T& operator()(size_t row, size_t col) const - { - assert(row < m && col < n); - - //return e[col * m + row]; - return e[col + row * m]; - } - - /// \brief Negates the matrix. - /// - /// \return The negated matrix. - mat operator-() const - { - return neg(); - } - - /// \brief Multiplies the matrix by a scalar. - /// - /// \param[in] rhs The scalar. - /// \return The multiplied matrix. - template - mat& operator*=(const S& rhs) - { - for (size_t i = 0; i < m*n; i++) - e[i] *= rhs; - - return *this; - } - - /// \brief Multiplies the matrix by another matrix - /// - /// \param[in] rhs The other matrix. - /// \return The multiplied matrix. - template - mat& operator*(const mat& rhs) - { - // columns from the matrix must match rows of the input matrix - if (m != s) - { - return *this; - } - - size_t row = 0; - size_t col = 0; - size_t cell = 0; - - mat return_mat = zero(); - - // Remember, this matrix is stored in column order - for (size_t row_index = 0; row_index < r; row_index++) - { - for (size_t col_index = 0; col_index < n; col_index++) - { - - cell = row_index + (col_index * r); - - for (size_t cell_index = 0; cell_index < m; cell_index++) - { - // calculated for the cell in the current row - row = (col_index * m) + cell_index; - // calculated for the cell in the current column - col = (cell_index * r) + row_index; - return_mat.e[cell] += this->e[row] * rhs.e[col]; - } - } - } - - return return_mat; - } - - /// \brief Divides the matrix by a scalar. - /// - /// \param[in] rhs The scalar. - /// \return The divided matrix. - template - mat& operator/=(const T & rhs) - { - for (size_t i = 0; i < m*n; i++) - e[i] /= rhs; - - return *this; - } - - /// \brief Adds a matrix to this matrix. - /// - /// \param[in] rhs The right-side vector. - /// \return The resulting vector. - template - mat& operator+=(const mat& rhs) - { - for (size_t i = 0; i < m*n; i++) - e[i] += rhs.e[i]; - - return *this; - } - - /// \brief Subtracts a matrix from this matrix. - /// - /// \param[in] rhs The right-side matrix. - /// \return The resulting matrix. - template - mat& operator-=(const mat& rhs) - { - for (size_t i = 0; i < m*n; i++) - e[i] -= rhs.e[i]; - - return *this; - } - - // Public Methods ///////////////////////////////////////////////////////// + /// \brief Indexing into the matrix's elements. + /// + /// \param[in] row The 0-based index row. + /// \param[in] col The 0-based index column. + /// \return The requested element. + T & operator()(size_t row, size_t col) + { + return const_cast(static_cast(*this)(row, col)); + } + + /// \brief Indexing into the matrix's elements. + /// + /// \param[in] row The 0-based index row. + /// \param[in] col The 0-based index column. + /// \return The requested element. + const T & operator()(size_t row, size_t col) const + { + assert(row < m && col < n); + + //return e[col * m + row]; + return e[col + row * m]; + } + + /// \brief Negates the matrix. + /// + /// \return The negated matrix. + mat operator-() const { return neg(); } + + /// \brief Multiplies the matrix by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The multiplied matrix. + template + mat & operator*=(const S & rhs) + { + for (size_t i = 0; i < m * n; i++) e[i] *= rhs; + + return *this; + } + + /// \brief Multiplies the matrix by another matrix + /// + /// \param[in] rhs The other matrix. + /// \return The multiplied matrix. + template + mat & operator*(const mat & rhs) + { + // columns from the matrix must match rows of the input matrix + if (m != s) { + return *this; + } + + size_t row = 0; + size_t col = 0; + size_t cell = 0; + + mat return_mat = zero(); + + // Remember, this matrix is stored in column order + for (size_t row_index = 0; row_index < r; row_index++) { + for (size_t col_index = 0; col_index < n; col_index++) { + cell = row_index + (col_index * r); + + for (size_t cell_index = 0; cell_index < m; cell_index++) { + // calculated for the cell in the current row + row = (col_index * m) + cell_index; + // calculated for the cell in the current column + col = (cell_index * r) + row_index; + return_mat.e[cell] += this->e[row] * rhs.e[col]; + } + } + } + + return return_mat; + } + + /// \brief Divides the matrix by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The divided matrix. + template + mat & operator/=(const T & rhs) + { + for (size_t i = 0; i < m * n; i++) e[i] /= rhs; + + return *this; + } + + /// \brief Adds a matrix to this matrix. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + template + mat & operator+=(const mat & rhs) + { + for (size_t i = 0; i < m * n; i++) e[i] += rhs.e[i]; + + return *this; + } + + /// \brief Subtracts a matrix from this matrix. + /// + /// \param[in] rhs The right-side matrix. + /// \return The resulting matrix. + template + mat & operator-=(const mat & rhs) + { + for (size_t i = 0; i < m * n; i++) e[i] -= rhs.e[i]; + + return *this; + } + + // Public Methods ///////////////////////////////////////////////////////// public: - - /// \brief The matrix's row dimension. - /// - /// \return The matrix's row dimension. - size_t dimRow() const { return m; } - - /// \brief The matrix's column dimension. - /// - /// \return The matrix's column dimension. - size_t dimCol() const { return n; } - - /// \brief Negates the matrix. - /// - /// \return The negated matrix. - mat neg() const - { - mat nm; - - for (size_t i = 0; i < m * n; i++) - nm.e[i] = -e[i]; - - return nm; - } - - /// \brief Multiplies the matrix by a scalar. - /// - /// \param[in] scalar The scalar value. - /// \return The multiplied matrix. - mat mult(const double& scalar) const - { - mat nm; - - for (size_t i = 0; i < m*n; i++) - nm.e[i] = e[i] * scalar; - - return nm; - } - - /// \brief Divides the matrix by a scalar. - /// - /// \param[in] scalar The scalar value. - /// \return The divided matrix. - mat div(const double& scalar) const - { - mat nm; - - for (size_t i = 0; i < m*n; i++) - nm.e[i] = e[i] / scalar; - - return nm; - } - - /// \brief Adds a matrix to this matrix. - /// - /// \param[in] toAdd The matrix to add. - /// \return The resulting matrix. - mat add(const mat& toAdd) const - { - mat nm; - - for (size_t i = 0; i < m*n; i++) - nm.e[i] = e[i] + toAdd.e[i]; - - return nm; - } - - /// \brief Subtracts a matrix from this matrix. - /// - /// \param[in] toSub The matrix to subtract from this. - /// \return The resulting matrix. - mat sub(const mat& toSub) const - { - mat nm; - - for (size_t i = 0; i < m*n; i++) - nm.e[i] = e[i] - toSub.e[i]; - - return nm; - } - - /// \brief Transposes the matrix. - /// - /// \return The computed transpose. - mat transpose() const - { - mat nm; - - for (size_t row = 0; row < m; row++) - { - for (size_t col = 0; col < n; col++) - { - nm.e[row * n + col] = e[col * m + row]; - } - } - - return nm; - } - + /// \brief The matrix's row dimension. + /// + /// \return The matrix's row dimension. + size_t dimRow() const { return m; } + + /// \brief The matrix's column dimension. + /// + /// \return The matrix's column dimension. + size_t dimCol() const { return n; } + + /// \brief Negates the matrix. + /// + /// \return The negated matrix. + mat neg() const + { + mat nm; + + for (size_t i = 0; i < m * n; i++) nm.e[i] = -e[i]; + + return nm; + } + + /// \brief Multiplies the matrix by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The multiplied matrix. + mat mult(const double & scalar) const + { + mat nm; + + for (size_t i = 0; i < m * n; i++) nm.e[i] = e[i] * scalar; + + return nm; + } + + /// \brief Divides the matrix by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The divided matrix. + mat div(const double & scalar) const + { + mat nm; + + for (size_t i = 0; i < m * n; i++) nm.e[i] = e[i] / scalar; + + return nm; + } + + /// \brief Adds a matrix to this matrix. + /// + /// \param[in] toAdd The matrix to add. + /// \return The resulting matrix. + mat add(const mat & toAdd) const + { + mat nm; + + for (size_t i = 0; i < m * n; i++) nm.e[i] = e[i] + toAdd.e[i]; + + return nm; + } + + /// \brief Subtracts a matrix from this matrix. + /// + /// \param[in] toSub The matrix to subtract from this. + /// \return The resulting matrix. + mat sub(const mat & toSub) const + { + mat nm; + + for (size_t i = 0; i < m * n; i++) nm.e[i] = e[i] - toSub.e[i]; + + return nm; + } + + /// \brief Transposes the matrix. + /// + /// \return The computed transpose. + mat transpose() const + { + mat nm; + + for (size_t row = 0; row < m; row++) { + for (size_t col = 0; col < n; col++) { + nm.e[row * n + col] = e[col * m + row]; + } + } + + return nm; + } }; // Specializations //////////////////////////////////////////////////////////// #if defined(_MSC_VER) - #pragma warning(push) - - // Disable warning about 'nonstandard extension used : nameless struct/union'. - #pragma warning(disable:4201) +#pragma warning(push) + +// Disable warning about 'nonstandard extension used : nameless struct/union'. +#pragma warning(disable : 4201) #endif /// \brief 2x2 matrix specialization. template struct mat<2, 2, T> { - - // Public Members ///////////////////////////////////////////////////////// + // Public Members ///////////////////////////////////////////////////////// public: + union { + struct + { + /// \brief Element 0,0. + T e00; - union - { - struct - { - /// \brief Element 0,0. - T e00; + /// \brief Element 1,0. + T e10; - /// \brief Element 1,0. - T e10; + /// \brief Element 0,1. + T e01; - /// \brief Element 0,1. - T e01; + /// \brief Element 1,1. + T e11; + }; - /// \brief Element 1,1. - T e11; - }; + /// \brief The matrix's elements. + T e[2 * 2]; + }; - /// \brief The matrix's elements. - T e[2 * 2]; - }; + // Constructors /////////////////////////////////////////////////////////// - // Constructors /////////////////////////////////////////////////////////// +public: + /// \brief Creates a new matrix with uninitialized elements. + mat() {} + + /// \brief Creates a new matrix with its elements initialized to val. + /// + /// \param[in] val The initialization value. + explicit mat(T val) : e00(val), e10(val), e01(val), e11(val) {} + + /// \brief Creates a new matrix with its components initialized to the + /// provided values. + /// + /// \param[in] e00v Element 0,0 value. + /// \param[in] e01v Element 0,1 value. + /// \param[in] e10v Element 1,0 value. + /// \param[in] e11v Element 1,1 value. + mat(T e00v, T e01v, T e10v, T e11v) : e00(e00v), e10(e10v), e01(e01v), e11(e11v) {} + + /// \brief Constructs a matrix from 4 column vectors. + /// + /// \param[in] col0 The 0 column vector. + /// \param[in] col1 The 1 column vector. + /// \param[in] col2 The 2 column vector. + /// \param[in] col3 The 3 column vector. + mat(vec<2, T> col0, vec<2, T> col1) : e00(col0.x), e10(col1.x), e01(col0.y), e11(col1.y) {} + + // Helper Methods ///////////////////////////////////////////////////////// public: + /// \brief Matrix with all of its elements set to 0. + /// + /// \return The 0 matrix. + static mat zero() { return mat<2, 2, T>(0); } - /// \brief Creates a new matrix with uninitialized elements. - mat() { } - - /// \brief Creates a new matrix with its elements initialized to val. - /// - /// \param[in] val The initialization value. - explicit mat(T val) : - e00(val), e10(val), - e01(val), e11(val) - { } - - /// \brief Creates a new matrix with its components initialized to the - /// provided values. - /// - /// \param[in] e00v Element 0,0 value. - /// \param[in] e01v Element 0,1 value. - /// \param[in] e10v Element 1,0 value. - /// \param[in] e11v Element 1,1 value. - mat(T e00v, T e01v, - T e10v, T e11v) : - e00(e00v), e10(e10v), - e01(e01v), e11(e11v) - { } - - /// \brief Constructs a matrix from 4 column vectors. - /// - /// \param[in] col0 The 0 column vector. - /// \param[in] col1 The 1 column vector. - /// \param[in] col2 The 2 column vector. - /// \param[in] col3 The 3 column vector. - mat(vec<2, T> col0, vec<2, T> col1) : - e00(col0.x), e10(col1.x), - e01(col0.y), e11(col1.y) - {} - - // Helper Methods ///////////////////////////////////////////////////////// + /// \brief Matrix with all of its elements set to 1. + /// + /// \return The 1 matrix. + static mat one() { return mat<2, 2, T>(1); } -public: + /// \brief Identity matrix with its diagonal elements set to 1. + /// + /// \return The identity matrix. + static mat<2, 2, T> identity() { return mat<2, 2, T>(1, 0, 0, 1); } - /// \brief Matrix with all of its elements set to 0. - /// - /// \return The 0 matrix. - static mat zero() - { - return mat<2, 2, T>(0); - } - - /// \brief Matrix with all of its elements set to 1. - /// - /// \return The 1 matrix. - static mat one() - { - return mat<2, 2, T>(1); - } - - /// \brief Identity matrix with its diagonal elements set to 1. - /// - /// \return The identity matrix. - static mat<2, 2, T> identity() - { - return mat<2, 2, T>( - 1, 0, - 0, 1); - } - - // Operator Overloads ///////////////////////////////////////////////////// + // Operator Overloads ///////////////////////////////////////////////////// public: + /// \brief Indexing into the matrix's elements. + /// + /// \param[in] row The 0-based index row. + /// \param[in] col The 0-based index column. + /// \return The requested element. + T & operator()(size_t row, size_t col) + { + return const_cast(static_cast(*this)(row, col)); + } + + /// \brief Indexing into the matrix's elements. + /// + /// \param[in] row The 0-based index row. + /// \param[in] col The 0-based index column. + /// \return The requested element. + const T & operator()(size_t row, size_t col) const + { + assert(row < 2 && col < 2); + + return e[col * 2 + row]; + } + + /// \brief Negates the matrix. + /// + /// \return The negated matrix. + mat operator-() const { return neg(); } + + /// \brief Multiplies the matrix by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The multiplied matrix. + template + mat & operator*=(const S & rhs) + { +#if defined(_MSC_VER) +#pragma warning(push) - /// \brief Indexing into the matrix's elements. - /// - /// \param[in] row The 0-based index row. - /// \param[in] col The 0-based index column. - /// \return The requested element. - T& operator()(size_t row, size_t col) - { - return const_cast(static_cast(*this)(row, col)); - } - - /// \brief Indexing into the matrix's elements. - /// - /// \param[in] row The 0-based index row. - /// \param[in] col The 0-based index column. - /// \return The requested element. - const T& operator()(size_t row, size_t col) const - { - assert(row < 2 && col < 2); - - return e[col * 2 + row]; - } - - /// \brief Negates the matrix. - /// - /// \return The negated matrix. - mat operator-() const - { - return neg(); - } - - /// \brief Multiplies the matrix by a scalar. - /// - /// \param[in] rhs The scalar. - /// \return The multiplied matrix. - template - mat& operator*=(const S& rhs) - { - #if defined(_MSC_VER) - #pragma warning(push) - - // The operator*= throws a warning when a mat*f is multiplied by a mat*d. - #pragma warning(disable:4244) - #endif - - for (size_t i = 0; i < 2 * 2; i++) - e[i] *= rhs; - - #if defined (_MSC_VER) - #pragma warning(pop) - #endif - - return *this; - } - - /// \brief Multiplies the matrix by another matrix. - /// - /// \param[in] rhs The other matrix. - /// \return The multiplied matrix. - template - mat& operator*=(const mat<2, 2, S>& rhs) - { - size_t row = 0; - size_t col = 0; - size_t cell = 0; - - mat<2, 2, T> return_mat = zero(); - - // Remember, this matrix is stored in column order - for (size_t row_index = 0; row_index < 2; row_index++) - { - for (size_t col_index = 0; col_index < 2; col_index++) - { - cell = row_index + (col_index * 2); - - for (size_t cell_index = 0; cell_index < 2; cell_index++) - { - // calculated for the cell in the current row - row = row_index + (cell_index * 2); - // calculated for the cell in the current column - col = cell_index + (col_index * 2); - - return_mat.e[cell] += this->e[row] * rhs.e[col]; - } - } - } - - *this = return_mat; - - return *this; - } - - /// \brief Divides the matrix by a scalar. - /// - /// \param[in] rhs The scalar. - /// \return The divided matrix. - template - mat& operator/=(const S& rhs) - { - #if defined(_MSC_VER) - #pragma warning(push) - - // The operator/= throws a warning when a mat*f is divided by a double. - #pragma warning(disable:4244) - #endif - - for (size_t i = 0; i < 2 * 2; i++) - e[i] /= rhs; - - #if defined (_MSC_VER) - #pragma warning(pop) - #endif - - return *this; - } - - /// \brief Adds a matrix to this matrix. - /// - /// \param[in] rhs The right-side vector. - /// \return The resulting vector. - template - mat& operator+=(const mat<2, 2, S>& rhs) - { - for (size_t i = 0; i < 2 * 2; i++) - e[i] += rhs.e[i]; - - return *this; - } - - /// \brief Subtracts a matrix from this matrix. - /// - /// \param[in] rhs The right-side matrix. - /// \return The resulting matrix. - template - mat& operator-=(const mat<2, 2, S>& rhs) - { - for (size_t i = 0; i < 2 * 2; i++) - e[i] -= rhs.e[i]; - - return *this; - } - - // Public Methods ///////////////////////////////////////////////////////// +// The operator*= throws a warning when a mat*f is multiplied by a mat*d. +#pragma warning(disable : 4244) +#endif -public: + for (size_t i = 0; i < 2 * 2; i++) e[i] *= rhs; + +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + + return *this; + } + + /// \brief Multiplies the matrix by another matrix. + /// + /// \param[in] rhs The other matrix. + /// \return The multiplied matrix. + template + mat & operator*=(const mat<2, 2, S> & rhs) + { + size_t row = 0; + size_t col = 0; + size_t cell = 0; + + mat<2, 2, T> return_mat = zero(); + + // Remember, this matrix is stored in column order + for (size_t row_index = 0; row_index < 2; row_index++) { + for (size_t col_index = 0; col_index < 2; col_index++) { + cell = row_index + (col_index * 2); + + for (size_t cell_index = 0; cell_index < 2; cell_index++) { + // calculated for the cell in the current row + row = row_index + (cell_index * 2); + // calculated for the cell in the current column + col = cell_index + (col_index * 2); + + return_mat.e[cell] += this->e[row] * rhs.e[col]; + } + } + } + + *this = return_mat; + + return *this; + } + + /// \brief Divides the matrix by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The divided matrix. + template + mat & operator/=(const S & rhs) + { +#if defined(_MSC_VER) +#pragma warning(push) - /// \brief The matrix's row dimension. - /// - /// \return The matrix's row dimension. - size_t dimRow() const { return 2; } - - /// \brief The matrix's column dimension. - /// - /// \return The matrix's column dimension. - - size_t dimCol() const { return 2; } - - template - void copy(const mat<2, 2, S> rhs) - { - *this = rhs; - } - - /// \brief Negates the matrix. - /// - /// \return The negated matrix. - mat neg() const - { - mat nm; - - for (size_t i = 0; i < 2 * 2; i++) - nm.e[i] = -e[i]; - - return nm; - } - - /// \brief Multiplies the matrix by a scalar. - /// - /// \param[in] scalar The scalar value. - /// \return The multiplied matrix. - mat mult(const double& scalar) const - { - mat nm; - - for (size_t i = 0; i < 2 * 2; i++) - nm.e[i] = e[i] * scalar; - - return nm; - } - - /// \brief Divides the matrix by a scalar. - /// - /// \param[in] scalar The scalar value. - /// \return The divided matrix. - mat div(const double& scalar) const - { - mat nm; - - for (size_t i = 0; i < 2 * 2; i++) - nm.e[i] = e[i] / scalar; - - return nm; - } - - /// \brief Adds a matrix to this matrix. - /// - /// \param[in] toAdd The matrix to add. - /// \return The resulting matrix. - mat add(const mat& toAdd) const - { - mat nm; - - for (size_t i = 0; i < 2 * 2; i++) - nm.e[i] = e[i] + toAdd.e[i]; - - return nm; - } - - /// \brief Subtracts a matrix from this matrix. - /// - /// \param[in] toSub The matrix to subtract from this. - /// \return The resulting matrix. - mat sub(const mat& toSub) const - { - mat nm; - - for (size_t i = 0; i < 2 * 2; i++) - nm.e[i] = e[i] - toSub.e[i]; - - return nm; - } - - /// \brief Transposes the matrix. - /// - /// \return The computed transpose. - mat<2, 2, T> transpose() const - { - mat<2, 2, T> nm; - - for (size_t row = 0; row < 2; row++) - { - for (size_t col = 0; col < 2; col++) - { - nm.e[row * 2 + col] = e[col * 2 + row]; - } - } - - return nm; - } +// The operator/= throws a warning when a mat*f is divided by a double. +#pragma warning(disable : 4244) +#endif + + for (size_t i = 0; i < 2 * 2; i++) e[i] /= rhs; + +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + + return *this; + } + + /// \brief Adds a matrix to this matrix. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + template + mat & operator+=(const mat<2, 2, S> & rhs) + { + for (size_t i = 0; i < 2 * 2; i++) e[i] += rhs.e[i]; + + return *this; + } + + /// \brief Subtracts a matrix from this matrix. + /// + /// \param[in] rhs The right-side matrix. + /// \return The resulting matrix. + template + mat & operator-=(const mat<2, 2, S> & rhs) + { + for (size_t i = 0; i < 2 * 2; i++) e[i] -= rhs.e[i]; + + return *this; + } + + // Public Methods ///////////////////////////////////////////////////////// + +public: + /// \brief The matrix's row dimension. + /// + /// \return The matrix's row dimension. + size_t dimRow() const { return 2; } + + /// \brief The matrix's column dimension. + /// + /// \return The matrix's column dimension. + + size_t dimCol() const { return 2; } + + template + void copy(const mat<2, 2, S> rhs) + { + *this = rhs; + } + + /// \brief Negates the matrix. + /// + /// \return The negated matrix. + mat neg() const + { + mat nm; + + for (size_t i = 0; i < 2 * 2; i++) nm.e[i] = -e[i]; + + return nm; + } + + /// \brief Multiplies the matrix by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The multiplied matrix. + mat mult(const double & scalar) const + { + mat nm; + + for (size_t i = 0; i < 2 * 2; i++) nm.e[i] = e[i] * scalar; + + return nm; + } + + /// \brief Divides the matrix by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The divided matrix. + mat div(const double & scalar) const + { + mat nm; + + for (size_t i = 0; i < 2 * 2; i++) nm.e[i] = e[i] / scalar; + + return nm; + } + + /// \brief Adds a matrix to this matrix. + /// + /// \param[in] toAdd The matrix to add. + /// \return The resulting matrix. + mat add(const mat & toAdd) const + { + mat nm; + + for (size_t i = 0; i < 2 * 2; i++) nm.e[i] = e[i] + toAdd.e[i]; + + return nm; + } + + /// \brief Subtracts a matrix from this matrix. + /// + /// \param[in] toSub The matrix to subtract from this. + /// \return The resulting matrix. + mat sub(const mat & toSub) const + { + mat nm; + + for (size_t i = 0; i < 2 * 2; i++) nm.e[i] = e[i] - toSub.e[i]; + + return nm; + } + + /// \brief Transposes the matrix. + /// + /// \return The computed transpose. + mat<2, 2, T> transpose() const + { + mat<2, 2, T> nm; + + for (size_t row = 0; row < 2; row++) { + for (size_t col = 0; col < 2; col++) { + nm.e[row * 2 + col] = e[col * 2 + row]; + } + } + + return nm; + } }; /// \brief 3x3 matrix specialization. @@ -677,799 +598,775 @@ struct mat<2, 2, T> template struct mat<3, 3, T> { - - // Public Members ///////////////////////////////////////////////////////// + // Public Members ///////////////////////////////////////////////////////// public: + union { + struct + { + /// \brief Element 0,0. + T e00; - union - { - struct - { - /// \brief Element 0,0. - T e00; + /// \brief Element 1,0. + T e10; - /// \brief Element 1,0. - T e10; + /// \brief Element 2,0. + T e20; - /// \brief Element 2,0. - T e20; + /// \brief Element 0,1. + T e01; - /// \brief Element 0,1. - T e01; + /// \brief Element 1,1. + T e11; - /// \brief Element 1,1. - T e11; + /// \brief Element 2,1. + T e21; - /// \brief Element 2,1. - T e21; + /// \brief Element 0,2. + T e02; - /// \brief Element 0,2. - T e02; + /// \brief Element 1,2. + T e12; - /// \brief Element 1,2. - T e12; + /// \brief Element 2,2. + T e22; + }; - /// \brief Element 2,2. - T e22; - }; + /// \brief The matrix's elements. + T e[3 * 3]; + }; - /// \brief The matrix's elements. - T e[3 * 3]; - }; + // Constructors /////////////////////////////////////////////////////////// - // Constructors /////////////////////////////////////////////////////////// +public: + /// \brief Creates a new matrix with uninitialized elements. + mat() {} + + /// \brief Creates a new matrix with its elements initialized to val. + /// + /// \param[in] val The initialization value. + explicit mat(T val) + : e00(val), e10(val), e20(val), e01(val), e11(val), e21(val), e02(val), e12(val), e22(val) + { + } + + /// \brief Creates a new matrix with its components intialized to the + /// provided values. + /// + /// \param[in] e00v Element 0,0 value. + /// \param[in] e01v Element 0,1 value. + /// \param[in] e02v Element 0,2 value. + /// \param[in] e10v Element 1,0 value. + /// \param[in] e11v Element 1,1 value. + /// \param[in] e12v Element 1,2 value. + /// \param[in] e20v Element 2,0 value. + /// \param[in] e21v Element 2,1 value. + /// \param[in] e22v Element 2,2 value. + mat(T e00v, T e01v, T e02v, T e10v, T e11v, T e12v, T e20v, T e21v, T e22v) + : e00(e00v), + e10(e10v), + e20(e20v), + e01(e01v), + e11(e11v), + e21(e21v), + e02(e02v), + e12(e12v), + e22(e22v) + + { + } + + /// \brief Constructs a matrix from 3 column vectors. Vectors are stored in column order + /// + /// \param[in] col0 The 0 column vector. + /// \param[in] col1 The 1 column vector. + /// \param[in] col2 The 2 column vector. + mat(vec<3, T> col0, vec<3, T> col1, vec<3, T> col2) + : e00(col0.x), + e10(col0.y), + e20(col0.z), + e01(col1.x), + e11(col1.y), + e21(col1.z), + e02(col2.x), + e12(col2.y), + e22(col2.z) + { + } + + // Helper Methods ///////////////////////////////////////////////////////// public: + /// \brief Matrix with all of its elements set to 0. + /// + /// \return The 0 matrix. + static mat zero() { return mat<3, 3, T>(0); } - /// \brief Creates a new matrix with uninitialized elements. - mat() { } - - /// \brief Creates a new matrix with its elements initialized to val. - /// - /// \param[in] val The initialization value. - explicit mat(T val) : - e00(val), e10(val), e20(val), - e01(val), e11(val), e21(val), - e02(val), e12(val), e22(val) - { } - - /// \brief Creates a new matrix with its components intialized to the - /// provided values. - /// - /// \param[in] e00v Element 0,0 value. - /// \param[in] e01v Element 0,1 value. - /// \param[in] e02v Element 0,2 value. - /// \param[in] e10v Element 1,0 value. - /// \param[in] e11v Element 1,1 value. - /// \param[in] e12v Element 1,2 value. - /// \param[in] e20v Element 2,0 value. - /// \param[in] e21v Element 2,1 value. - /// \param[in] e22v Element 2,2 value. - mat(T e00v, T e01v, T e02v, - T e10v, T e11v, T e12v, - T e20v, T e21v, T e22v) : - e00(e00v), e10(e10v), e20(e20v), - e01(e01v), e11(e11v), e21(e21v), - e02(e02v), e12(e12v), e22(e22v) - - { } - - /// \brief Constructs a matrix from 3 column vectors. Vectors are stored in column order - /// - /// \param[in] col0 The 0 column vector. - /// \param[in] col1 The 1 column vector. - /// \param[in] col2 The 2 column vector. - mat(vec<3, T> col0, vec<3, T> col1, vec<3, T> col2) : - e00(col0.x), e10(col0.y), e20(col0.z), - e01(col1.x), e11(col1.y), e21(col1.z), - e02(col2.x), e12(col2.y), e22(col2.z) - { - - } - - // Helper Methods ///////////////////////////////////////////////////////// + /// \brief Matrix with all of its elements set to 1. + /// + /// \return The 1 matrix. + static mat one() { return mat<3, 3, T>(1); } -public: + /// \brief Identity matrix with its diagonal elements set to 1. + /// + /// \return The identity matrix. + static mat<3, 3, T> identity() { return mat<3, 3, T>(1, 0, 0, 0, 1, 0, 0, 0, 1); } - /// \brief Matrix with all of its elements set to 0. - /// - /// \return The 0 matrix. - static mat zero() - { - return mat<3, 3, T>(0); - } - - /// \brief Matrix with all of its elements set to 1. - /// - /// \return The 1 matrix. - static mat one() - { - return mat<3, 3, T>(1); - } - - /// \brief Identity matrix with its diagonal elements set to 1. - /// - /// \return The identity matrix. - static mat<3, 3, T> identity() - { - return mat<3, 3, T>( - 1, 0, 0, - 0, 1, 0, - 0, 0, 1); - } - - // Operator Overloads ///////////////////////////////////////////////////// + // Operator Overloads ///////////////////////////////////////////////////// public: + /// \brief Indexing into the matrix's elements. + /// + /// \param[in] row The 0-based index row. + /// \param[in] col The 0-based index column. + /// \return The requested element. + T & operator()(size_t row, size_t col) + { + return const_cast(static_cast(*this)(row, col)); + } + + /// \brief Indexing into the matrix's elements. + /// + /// \param[in] row The 0-based index row. + /// \param[in] col The 0-based index column. + /// \return The requested element. + const T & operator()(size_t row, size_t col) const + { + assert(row < 3 && col < 3); + + return e[col * 3 + row]; + } + + /// \brief Negates the matrix. + /// + /// \return The negated matrix. + mat operator-() const { return neg(); } + + /// \brief Multiplies the matrix by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The multiplied matrix. + template + mat & operator*=(const S & rhs) + { +#if defined(_MSC_VER) +#pragma warning(push) - /// \brief Indexing into the matrix's elements. - /// - /// \param[in] row The 0-based index row. - /// \param[in] col The 0-based index column. - /// \return The requested element. - T& operator()(size_t row, size_t col) - { - return const_cast(static_cast(*this)(row, col)); - } - - /// \brief Indexing into the matrix's elements. - /// - /// \param[in] row The 0-based index row. - /// \param[in] col The 0-based index column. - /// \return The requested element. - const T& operator()(size_t row, size_t col) const - { - assert(row < 3 && col < 3); - - return e[col * 3 + row]; - } - - /// \brief Negates the matrix. - /// - /// \return The negated matrix. - mat operator-() const - { - return neg(); - } - - /// \brief Multiplies the matrix by a scalar. - /// - /// \param[in] rhs The scalar. - /// \return The multiplied matrix. - template - mat& operator*=(const S& rhs) - { - #if defined(_MSC_VER) - #pragma warning(push) - - // The operator*= throws a warning when a mat*f is multiplied by a mat*d. - #pragma warning(disable:4244) - #endif - - for (size_t i = 0; i < 3 * 3; i++) - e[i] *= rhs; - - #if defined (_MSC_VER) - #pragma warning(pop) - #endif - - return *this; - } - - /// \brief Multiplies the matrix by another matrix. - /// - /// \param[in] rhs The other matrix. - /// \return The multiplied matrix. - template - mat& operator*=(const mat<3, 3, S>& rhs) - { - size_t counter = 0; - size_t row = 0; - size_t col = 0; - size_t cell = 0; - mat<3, 3, T> return_mat = zero(); - - // Remember, this matrix is stored in column order - for(size_t row_index = 0; row_index < 3; row_index++) - { - for(size_t col_index = 0; col_index < 3; col_index++) - { - cell = row_index + (col_index * 3); - - for (size_t cell_index = 0; cell_index < 3; cell_index++) - { - // calculated for the cell in the current row - row = row_index + (cell_index * 3); - // calculated for the cell in the current column - col = cell_index + (col_index * 3); - - return_mat.e[cell] += this->e[row] * rhs.e[col]; - } - } - } - - // std::copy(return_mat.e[0], return_mat.e[8], this->e[0]); - // std::copy(return_mat.e, return_mat.e + 7, this->e); - *this = return_mat; - - return *this; - } - - /// \brief Divides the matrix by a scalar. - /// - /// \param[in] rhs The scalar. - /// \return The divided matrix. - template - mat& operator/=(const S & rhs) - { - #if defined(_MSC_VER) - #pragma warning(push) - - // The operator/= throws a warning when a mat*f is divided by a double. - #pragma warning(disable:4244) - #endif - - for (size_t i = 0; i < 3 * 3; i++) - e[i] /= rhs; - - #if defined (_MSC_VER) - #pragma warning(pop) - #endif - - return *this; - } - - /// \brief Adds a matrix to this matrix. - /// - /// \param[in] rhs The right-side vector. - /// \return The resulting vector. - template - mat& operator+=(const mat<3, 3, S>& rhs) - { - for (size_t i = 0; i < 3 * 3; i++) - e[i] += rhs.e[i]; - - return *this; - } - - /// \brief Subtracts a matrix from this matrix. - /// - /// \param[in] rhs The right-side matrix. - /// \return The resulting matrix. - template - mat& operator-=(const mat<3, 3, S>& rhs) - { - for (size_t i = 0; i < 3 * 3; i++) - e[i] -= rhs.e[i]; - - return *this; - } - - // Public Methods ///////////////////////////////////////////////////////// +// The operator*= throws a warning when a mat*f is multiplied by a mat*d. +#pragma warning(disable : 4244) +#endif -public: + for (size_t i = 0; i < 3 * 3; i++) e[i] *= rhs; - /// \brief The matrix's row dimension. - /// - /// \return The matrix's row dimension. - size_t dimRow() const { return 3; } - - /// \brief The matrix's column dimension. - /// - /// \return The matrix's column dimension. - size_t dimCol() const { return 3; } - size_t dimCols() const { return 3; } - - template - void copy(const mat<3, 3, S>& rhs) - { - *this = rhs; - } - - /// \brief Negates the matrix. - /// - /// \return The negated matrix. - mat neg() const - { - mat nm; - - for (size_t i = 0; i < 3 * 3; i++) - nm.e[i] = -e[i]; - - return nm; - } - - /// \brief Multiplies the matrix by a scalar. - /// - /// \param[in] scalar The scalar value. - /// \return The multiplied matrix. - mat mult(const double& scalar) const - { - mat nm; - - for (size_t i = 0; i < 3 * 3; i++) - nm.e[i] = e[i] * scalar; - - return nm; - } - - /// \brief Divides the matrix by a scalar. - /// - /// \param[in] scalar The scalar value. - /// \return The divided matrix. - mat div(const double& scalar) const - { - mat nm; - - for (size_t i = 0; i < 3 * 3; i++) - nm.e[i] = e[i] / scalar; - - return nm; - } - - /// \brief Adds a matrix to this matrix. - /// - /// \param[in] toAdd The matrix to add. - /// \return The resulting matrix. - mat add(const mat& toAdd) const - { - mat nm; - - for (size_t i = 0; i < 3 * 3; i++) - nm.e[i] = e[i] + toAdd.e[i]; - - return nm; - } - - /// \brief Subtracts a matrix from this matrix. - /// - /// \param[in] toSub The matrix to subtract from this. - /// \return The resulting matrix. - mat sub(const mat& toSub) const - { - mat nm; - - for (size_t i = 0; i < 3 * 3; i++) - nm.e[i] = e[i] - toSub.e[i]; - - return nm; - } - - /// \brief Transposes the matrix. - /// - /// \return The computed transpose. - mat<3, 3, T> transpose() const - { - mat<3, 3, T> nm; - - for (size_t row = 0; row < 3; row++) - { - for (size_t col = 0; col < 3; col++) - { - nm.e[row * 3 + col] = e[col * 3 + row]; - } - } - - return nm; - } +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + + return *this; + } + + /// \brief Multiplies the matrix by another matrix. + /// + /// \param[in] rhs The other matrix. + /// \return The multiplied matrix. + template + mat & operator*=(const mat<3, 3, S> & rhs) + { + size_t counter = 0; + size_t row = 0; + size_t col = 0; + size_t cell = 0; + mat<3, 3, T> return_mat = zero(); + + // Remember, this matrix is stored in column order + for (size_t row_index = 0; row_index < 3; row_index++) { + for (size_t col_index = 0; col_index < 3; col_index++) { + cell = row_index + (col_index * 3); + + for (size_t cell_index = 0; cell_index < 3; cell_index++) { + // calculated for the cell in the current row + row = row_index + (cell_index * 3); + // calculated for the cell in the current column + col = cell_index + (col_index * 3); + + return_mat.e[cell] += this->e[row] * rhs.e[col]; + } + } + } + + // std::copy(return_mat.e[0], return_mat.e[8], this->e[0]); + // std::copy(return_mat.e, return_mat.e + 7, this->e); + *this = return_mat; + + return *this; + } + + /// \brief Divides the matrix by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The divided matrix. + template + mat & operator/=(const S & rhs) + { +#if defined(_MSC_VER) +#pragma warning(push) + +// The operator/= throws a warning when a mat*f is divided by a double. +#pragma warning(disable : 4244) +#endif + + for (size_t i = 0; i < 3 * 3; i++) e[i] /= rhs; + +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + + return *this; + } + + /// \brief Adds a matrix to this matrix. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + template + mat & operator+=(const mat<3, 3, S> & rhs) + { + for (size_t i = 0; i < 3 * 3; i++) e[i] += rhs.e[i]; + + return *this; + } + + /// \brief Subtracts a matrix from this matrix. + /// + /// \param[in] rhs The right-side matrix. + /// \return The resulting matrix. + template + mat & operator-=(const mat<3, 3, S> & rhs) + { + for (size_t i = 0; i < 3 * 3; i++) e[i] -= rhs.e[i]; + + return *this; + } + + // Public Methods ///////////////////////////////////////////////////////// + +public: + /// \brief The matrix's row dimension. + /// + /// \return The matrix's row dimension. + size_t dimRow() const { return 3; } + + /// \brief The matrix's column dimension. + /// + /// \return The matrix's column dimension. + size_t dimCol() const { return 3; } + size_t dimCols() const { return 3; } + + template + void copy(const mat<3, 3, S> & rhs) + { + *this = rhs; + } + + /// \brief Negates the matrix. + /// + /// \return The negated matrix. + mat neg() const + { + mat nm; + + for (size_t i = 0; i < 3 * 3; i++) nm.e[i] = -e[i]; + + return nm; + } + + /// \brief Multiplies the matrix by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The multiplied matrix. + mat mult(const double & scalar) const + { + mat nm; + + for (size_t i = 0; i < 3 * 3; i++) nm.e[i] = e[i] * scalar; + + return nm; + } + + /// \brief Divides the matrix by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The divided matrix. + mat div(const double & scalar) const + { + mat nm; + + for (size_t i = 0; i < 3 * 3; i++) nm.e[i] = e[i] / scalar; + + return nm; + } + + /// \brief Adds a matrix to this matrix. + /// + /// \param[in] toAdd The matrix to add. + /// \return The resulting matrix. + mat add(const mat & toAdd) const + { + mat nm; + + for (size_t i = 0; i < 3 * 3; i++) nm.e[i] = e[i] + toAdd.e[i]; + + return nm; + } + + /// \brief Subtracts a matrix from this matrix. + /// + /// \param[in] toSub The matrix to subtract from this. + /// \return The resulting matrix. + mat sub(const mat & toSub) const + { + mat nm; + + for (size_t i = 0; i < 3 * 3; i++) nm.e[i] = e[i] - toSub.e[i]; + + return nm; + } + + /// \brief Transposes the matrix. + /// + /// \return The computed transpose. + mat<3, 3, T> transpose() const + { + mat<3, 3, T> nm; + + for (size_t row = 0; row < 3; row++) { + for (size_t col = 0; col < 3; col++) { + nm.e[row * 3 + col] = e[col * 3 + row]; + } + } + + return nm; + } }; /// \brief 4x4 matrix specialization. template struct mat<4, 4, T> { - - // Public Members ///////////////////////////////////////////////////////// + // Public Members ///////////////////////////////////////////////////////// public: + union { + struct + { + /// \brief Element 0,0. + T e00; - union - { - struct - { - /// \brief Element 0,0. - T e00; - - /// \brief Element 1,0. - T e10; + /// \brief Element 1,0. + T e10; - /// \brief Element 2,0. - T e20; + /// \brief Element 2,0. + T e20; - /// \brief Element 3,0. - T e30; + /// \brief Element 3,0. + T e30; - /// \brief Element 0,1. - T e01; + /// \brief Element 0,1. + T e01; - /// \brief Element 1,1. - T e11; + /// \brief Element 1,1. + T e11; - /// \brief Element 2,1. - T e21; + /// \brief Element 2,1. + T e21; - /// \brief Element 3,1. - T e31; + /// \brief Element 3,1. + T e31; - /// \brief Element 0,2. - T e02; + /// \brief Element 0,2. + T e02; - /// \brief Element 1,2. - T e12; + /// \brief Element 1,2. + T e12; - /// \brief Element 2,2. - T e22; + /// \brief Element 2,2. + T e22; - /// \brief Element 3,2. - T e32; + /// \brief Element 3,2. + T e32; - /// \brief Element 0,3. - T e03; + /// \brief Element 0,3. + T e03; - /// \brief Element 1,3. - T e13; + /// \brief Element 1,3. + T e13; - /// \brief Element 2,3. - T e23; + /// \brief Element 2,3. + T e23; - /// \brief Element 3,3. - T e33; - }; + /// \brief Element 3,3. + T e33; + }; - /// \brief The matrix's elements. - T e[4*4]; - }; + /// \brief The matrix's elements. + T e[4 * 4]; + }; - // Constructors /////////////////////////////////////////////////////////// + // Constructors /////////////////////////////////////////////////////////// public: + /// \brief Creates a new matrix with uninitialized elements. + mat() {} + + /// \brief Creates a new matrix with its elements initialized to val. + /// + /// \param[in] val The initialization value. + explicit mat(T val) + : e00(val), + e01(val), + e02(val), + e03(val), + e10(val), + e11(val), + e12(val), + e13(val), + e20(val), + e21(val), + e22(val), + e23(val), + e30(val), + e31(val), + e32(val), + e33(val) + { + } + + /// \brief Creates a new matrix with its components intialized to the + /// provided values. + /// + /// \param[in] e00v Element 0,0 value. + /// \param[in] e01v Element 0,1 value. + /// \param[in] e02v Element 0,2 value. + /// \param[in] e03v Element 0,3 value. + /// \param[in] e10v Element 1,0 value. + /// \param[in] e11v Element 1,1 value. + /// \param[in] e12v Element 1,2 value. + /// \param[in] e13v Element 1,3 value. + /// \param[in] e20v Element 2,0 value. + /// \param[in] e21v Element 2,1 value. + /// \param[in] e22v Element 2,2 value. + /// \param[in] e23v Element 2,3 value. + /// \param[in] e30v Element 3,0 value. + /// \param[in] e31v Element 3,1 value. + /// \param[in] e32v Element 3,2 value. + /// \param[in] e33v Element 3,3 value. + mat( + T e00v, T e01v, T e02v, T e03v, T e10v, T e11v, T e12v, T e13v, T e20v, T e21v, T e22v, T e23v, + T e30v, T e31v, T e32v, T e33v) + : e00(e00v), + e01(e01v), + e02(e02v), + e03(e03v), + e10(e10v), + e11(e11v), + e12(e12v), + e13(e13v), + e20(e20v), + e21(e21v), + e22(e22v), + e23(e23v), + e30(e30v), + e31(e31v), + e32(e32v), + e33(e33v) + { + } + + /// \brief Constructs a matrix from 4 column vectors. + /// + /// \param[in] col0 The 0 column vector. + /// \param[in] col1 The 1 column vector. + /// \param[in] col2 The 2 column vector. + /// \param[in] col3 The 3 column vector. + mat(vec<4, T> col0, vec<4, T> col1, vec<4, T> col2, vec<4, T> col3) + : e00(col0.x), + e10(col1.x), + e20(col2.x), + e30(col3.x), + e01(col0.y), + e11(col1.y), + e21(col2.y), + e31(col3.y), + e02(col0.z), + e12(col1.z), + e22(col2.z), + e32(col3.z), + e03(col0.w), + e13(col1.w), + e23(col2.w), + e33(col3.w) + { + } + + // Helper Methods ///////////////////////////////////////////////////////// - /// \brief Creates a new matrix with uninitialized elements. - mat() { } - - /// \brief Creates a new matrix with its elements initialized to val. - /// - /// \param[in] val The initialization value. - explicit mat(T val) : - e00(val), e01(val), e02(val), e03(val), - e10(val), e11(val), e12(val), e13(val), - e20(val), e21(val), e22(val), e23(val), - e30(val), e31(val), e32(val), e33(val) - { } - - /// \brief Creates a new matrix with its components intialized to the - /// provided values. - /// - /// \param[in] e00v Element 0,0 value. - /// \param[in] e01v Element 0,1 value. - /// \param[in] e02v Element 0,2 value. - /// \param[in] e03v Element 0,3 value. - /// \param[in] e10v Element 1,0 value. - /// \param[in] e11v Element 1,1 value. - /// \param[in] e12v Element 1,2 value. - /// \param[in] e13v Element 1,3 value. - /// \param[in] e20v Element 2,0 value. - /// \param[in] e21v Element 2,1 value. - /// \param[in] e22v Element 2,2 value. - /// \param[in] e23v Element 2,3 value. - /// \param[in] e30v Element 3,0 value. - /// \param[in] e31v Element 3,1 value. - /// \param[in] e32v Element 3,2 value. - /// \param[in] e33v Element 3,3 value. - mat(T e00v, T e01v, T e02v, T e03v, - T e10v, T e11v, T e12v, T e13v, - T e20v, T e21v, T e22v, T e23v, - T e30v, T e31v, T e32v, T e33v) : - e00(e00v), e01(e01v), e02(e02v), e03(e03v), - e10(e10v), e11(e11v), e12(e12v), e13(e13v), - e20(e20v), e21(e21v), e22(e22v), e23(e23v), - e30(e30v), e31(e31v), e32(e32v), e33(e33v) - { } - - /// \brief Constructs a matrix from 4 column vectors. - /// - /// \param[in] col0 The 0 column vector. - /// \param[in] col1 The 1 column vector. - /// \param[in] col2 The 2 column vector. - /// \param[in] col3 The 3 column vector. - mat(vec<4, T> col0, vec<4, T> col1, vec<4, T> col2, vec<4, T> col3) : - e00(col0.x), e10(col1.x), e20(col2.x), e30(col3.x), - e01(col0.y), e11(col1.y), e21(col2.y), e31(col3.y), - e02(col0.z), e12(col1.z), e22(col2.z), e32(col3.z), - e03(col0.w), e13(col1.w), e23(col2.w), e33(col3.w) - { - - } - - // Helper Methods ///////////////////////////////////////////////////////// +public: + /// \brief Matrix with all of its elements set to 0. + /// + /// \return The 0 matrix. + static mat zero() { return mat<4, 4, T>(0); } + + /// \brief Matrix with all of its elements set to 1. + /// + /// \return The 1 matrix. + static mat one() { return mat<4, 4, T>(1); } + + /// \brief Identity matrix with its diagonal elements set to 1. + /// + /// \return The identity matrix. + static mat<4, 4, T> identity() + { + return mat<4, 4, T>(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); + } + + // Operator Overloads ///////////////////////////////////////////////////// public: + /// \brief Indexing into the matrix's elements. + /// + /// \param[in] row The 0-based index row. + /// \param[in] col The 0-based index column. + /// \return The requested element. + T & operator()(size_t row, size_t col) + { + return const_cast(static_cast(*this)(row, col)); + } + + /// \brief Indexing into the matrix's elements. + /// + /// \param[in] row The 0-based index row. + /// \param[in] col The 0-based index column. + /// \return The requested element. + const T & operator()(size_t row, size_t col) const + { + assert(row < 4 && col < 4); + + return e[col * 4 + row]; + } + + /// \brief Negates the matrix. + /// + /// \return The negated matrix. + mat operator-() const { return neg(); } + + /// \brief Multiplies the matrix by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The multiplied matrix. + template + mat & operator*=(const S & rhs) + { +#if defined(_MSC_VER) +#pragma warning(push) + +// The operator*= throws a warning when a mat*f is multiplied by a mat*d. +#pragma warning(disable : 4244) +#endif - /// \brief Matrix with all of its elements set to 0. - /// - /// \return The 0 matrix. - static mat zero() - { - return mat<4, 4, T>(0); - } - - /// \brief Matrix with all of its elements set to 1. - /// - /// \return The 1 matrix. - static mat one() - { - return mat<4, 4, T>(1); - } - - /// \brief Identity matrix with its diagonal elements set to 1. - /// - /// \return The identity matrix. - static mat<4, 4, T> identity() - { - return mat<4, 4, T>(1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1); - } - - // Operator Overloads ///////////////////////////////////////////////////// + for (size_t i = 0; i < 4 * 4; i++) e[i] *= rhs; -public: +#if defined(_MSC_VER) +#pragma warning(pop) +#endif - /// \brief Indexing into the matrix's elements. - /// - /// \param[in] row The 0-based index row. - /// \param[in] col The 0-based index column. - /// \return The requested element. - T& operator()(size_t row, size_t col) - { - return const_cast(static_cast(*this)(row, col)); - } - - /// \brief Indexing into the matrix's elements. - /// - /// \param[in] row The 0-based index row. - /// \param[in] col The 0-based index column. - /// \return The requested element. - const T& operator()(size_t row, size_t col) const - { - assert(row < 4 && col < 4); - - return e[col * 4 + row]; - } - - /// \brief Negates the matrix. - /// - /// \return The negated matrix. - mat operator-() const - { - return neg(); - } - - /// \brief Multiplies the matrix by a scalar. - /// - /// \param[in] rhs The scalar. - /// \return The multiplied matrix. - template - mat& operator*=(const S& rhs) - { - #if defined(_MSC_VER) - #pragma warning(push) - - // The operator*= throws a warning when a mat*f is multiplied by a mat*d. - #pragma warning(disable:4244) - #endif - - for (size_t i = 0; i < 4 * 4; i++) - e[i] *= rhs; - - #if defined (_MSC_VER) - #pragma warning(pop) - #endif - - return *this; - } - - /// \brief Multiplies the matrix by another matrix. - /// - /// \param[in] rhs The other matrix. - /// \return The multiplied matrix. - template - mat& operator*=(const mat<4, 4, S>& rhs) - { - size_t row = 0; - size_t col = 0; - size_t cell = 0; - - mat<4, 4, T> return_mat = zero(); - - // Remember, this matrix is stored in column order - for (size_t row_index = 0; row_index < 4; row_index++) - { - for (size_t col_index = 0; col_index < 4; col_index++) - { - cell = row_index + (col_index * 4); - - for (size_t cell_index = 0; cell_index < 4; cell_index++) - { - // calculated for the cell in the current row - row = row_index + (cell_index * 4); - // calculated for the cell in the current column - col = cell_index + (col_index * 4); - - return_mat.e[cell] += this->e[row] * rhs.e[col]; - } - } - } - - *this = return_mat; - - return *this; - } - - /// \brief Divides the matrix by a scalar. - /// - /// \param[in] rhs The scalar. - /// \return The divided matrix. - template - mat& operator/=(const S& rhs) - { - #if defined(_MSC_VER) - #pragma warning(push) - - // The operator/= throws a warning when a mat*f is divided by a double. - #pragma warning(disable:4244) - #endif - - for (size_t i = 0; i < 4 * 4; i++) - e[i] /= rhs; - - #if defined (_MSC_VER) - #pragma warning(pop) - #endif - - return *this; - } - - /// \brief Adds a matrix to this matrix. - /// - /// \param[in] rhs The right-side vector. - /// \return The resulting vector. - template - mat& operator+=(const mat<4, 4, S>& rhs) - { - for (size_t i = 0; i < 4*4; i++) - e[i] += rhs.e[i]; - - return *this; - } - - /// \brief Subtracts a matrix from this matrix. - /// - /// \param[in] rhs The right-side matrix. - /// \return The resulting matrix. - template - mat& operator-=(const mat<4, 4, S>& rhs) - { - for (size_t i = 0; i < 4*4; i++) - e[i] -= rhs.e[i]; - - return *this; - } - - // Public Methods ///////////////////////////////////////////////////////// + return *this; + } + + /// \brief Multiplies the matrix by another matrix. + /// + /// \param[in] rhs The other matrix. + /// \return The multiplied matrix. + template + mat & operator*=(const mat<4, 4, S> & rhs) + { + size_t row = 0; + size_t col = 0; + size_t cell = 0; + + mat<4, 4, T> return_mat = zero(); + + // Remember, this matrix is stored in column order + for (size_t row_index = 0; row_index < 4; row_index++) { + for (size_t col_index = 0; col_index < 4; col_index++) { + cell = row_index + (col_index * 4); + + for (size_t cell_index = 0; cell_index < 4; cell_index++) { + // calculated for the cell in the current row + row = row_index + (cell_index * 4); + // calculated for the cell in the current column + col = cell_index + (col_index * 4); + + return_mat.e[cell] += this->e[row] * rhs.e[col]; + } + } + } + + *this = return_mat; + + return *this; + } + + /// \brief Divides the matrix by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The divided matrix. + template + mat & operator/=(const S & rhs) + { +#if defined(_MSC_VER) +#pragma warning(push) -public: +// The operator/= throws a warning when a mat*f is divided by a double. +#pragma warning(disable : 4244) +#endif + + for (size_t i = 0; i < 4 * 4; i++) e[i] /= rhs; + +#if defined(_MSC_VER) +#pragma warning(pop) +#endif - /// \brief The matrix's row dimension. - /// - /// \return The matrix's row dimension. - size_t dimRow() const { return 4; } - - /// \brief The matrix's column dimension. - /// - /// \return The matrix's column dimension. - size_t dimCol() const { return 4; } - - template - void copy(const mat<4, 4, S>& rhs) - { - *this = rhs; - } - - /// \brief Negates the matrix. - /// - /// \return The negated matrix. - mat neg() const - { - mat nm; - - for (size_t i = 0; i < 4 * 4; i++) - nm.e[i] = -e[i]; - - return nm; - } - - /// \brief Multiplies the matrix by a scalar. - /// - /// \param[in] scalar The scalar value. - /// \return The multiplied matrix. - mat mult(const double& scalar) const - { - mat nm; - - for (size_t i = 0; i < 4*4; i++) - nm.e[i] = e[i] * scalar; - - return nm; - } - - //mat mult(const mat& rhs) - - /// \brief Divides the matrix by a scalar. - /// - /// \param[in] scalar The scalar value. - /// \return The divided matrix. - mat div(const double& scalar) const - { - mat nm; - - for (size_t i = 0; i < 4*4; i++) - nm.e[i] = e[i] / scalar; - - return nm; - } - - /// \brief Adds a matrix to this matrix. - /// - /// \param[in] toAdd The matrix to add. - /// \return The resulting matrix. - mat add(const mat& toAdd) const - { - mat nm; - - for (size_t i = 0; i < 4*4; i++) - nm.e[i] = e[i] + toAdd.e[i]; - - return nm; - } - - /// \brief Subtracts a matrix from this matrix. - /// - /// \param[in] toSub The matrix to subtract from this. - /// \return The resulting matrix. - mat sub(const mat& toSub) const - { - mat nm; - - for (size_t i = 0; i < 4*4; i++) - nm.e[i] = e[i] - toSub.e[i]; - - return nm; - } - - /// \brief Transposes the matrix. - /// - /// \return The computed transpose. - mat<4, 4, T> transpose() const - { - mat<4, 4, T> nm; - - for (size_t row = 0; row < 4; row++) - { - for (size_t col = 0; col < 4; col++) - { - nm.e[row * 4 + col] = e[col * 4 + row]; - } - } - - return nm; - } + return *this; + } + + /// \brief Adds a matrix to this matrix. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + template + mat & operator+=(const mat<4, 4, S> & rhs) + { + for (size_t i = 0; i < 4 * 4; i++) e[i] += rhs.e[i]; + + return *this; + } + + /// \brief Subtracts a matrix from this matrix. + /// + /// \param[in] rhs The right-side matrix. + /// \return The resulting matrix. + template + mat & operator-=(const mat<4, 4, S> & rhs) + { + for (size_t i = 0; i < 4 * 4; i++) e[i] -= rhs.e[i]; + + return *this; + } + + // Public Methods ///////////////////////////////////////////////////////// + +public: + /// \brief The matrix's row dimension. + /// + /// \return The matrix's row dimension. + size_t dimRow() const { return 4; } + + /// \brief The matrix's column dimension. + /// + /// \return The matrix's column dimension. + size_t dimCol() const { return 4; } + + template + void copy(const mat<4, 4, S> & rhs) + { + *this = rhs; + } + + /// \brief Negates the matrix. + /// + /// \return The negated matrix. + mat neg() const + { + mat nm; + + for (size_t i = 0; i < 4 * 4; i++) nm.e[i] = -e[i]; + + return nm; + } + + /// \brief Multiplies the matrix by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The multiplied matrix. + mat mult(const double & scalar) const + { + mat nm; + + for (size_t i = 0; i < 4 * 4; i++) nm.e[i] = e[i] * scalar; + + return nm; + } + + //mat mult(const mat& rhs) + + /// \brief Divides the matrix by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The divided matrix. + mat div(const double & scalar) const + { + mat nm; + + for (size_t i = 0; i < 4 * 4; i++) nm.e[i] = e[i] / scalar; + + return nm; + } + + /// \brief Adds a matrix to this matrix. + /// + /// \param[in] toAdd The matrix to add. + /// \return The resulting matrix. + mat add(const mat & toAdd) const + { + mat nm; + + for (size_t i = 0; i < 4 * 4; i++) nm.e[i] = e[i] + toAdd.e[i]; + + return nm; + } + + /// \brief Subtracts a matrix from this matrix. + /// + /// \param[in] toSub The matrix to subtract from this. + /// \return The resulting matrix. + mat sub(const mat & toSub) const + { + mat nm; + + for (size_t i = 0; i < 4 * 4; i++) nm.e[i] = e[i] - toSub.e[i]; + + return nm; + } + + /// \brief Transposes the matrix. + /// + /// \return The computed transpose. + mat<4, 4, T> transpose() const + { + mat<4, 4, T> nm; + + for (size_t row = 0; row < 4; row++) { + for (size_t col = 0; col < 4; col++) { + nm.e[row * 4 + col] = e[col * 4 + row]; + } + } + + return nm; + } }; -#if defined (_MSC_VER) - #pragma warning(pop) +#if defined(_MSC_VER) +#pragma warning(pop) #endif // Operator Overloads ///////////////////////////////////////////////////////// @@ -1480,11 +1377,11 @@ struct mat<4, 4, T> /// \param[in] rhs The scalar. /// \return The result. template -mat operator*(mat lhs, const S& rhs) +mat operator*(mat lhs, const S & rhs) { - lhs *= rhs; + lhs *= rhs; - return lhs; + return lhs; } /// \brief Multiplies a matrix by a scalar. @@ -1493,11 +1390,11 @@ mat operator*(mat lhs, const S& rhs) /// \param[in] rhs The matrix. /// \return The result. template -mat operator*(const S& lhs, mat rhs) +mat operator*(const S & lhs, mat rhs) { - rhs *= lhs; + rhs *= lhs; - return rhs; + return rhs; } /// \brief Multiplies two matrices together. @@ -1506,12 +1403,12 @@ mat operator*(const S& lhs, mat rhs) /// \param[in] rhs The right-side matrix. /// \return The result. template -mat operator*(mat&lhs, const mat& rhs) +mat operator*(mat & lhs, const mat & rhs) { - mat tmp = lhs; - tmp *= rhs; + mat tmp = lhs; + tmp *= rhs; - return tmp; + return tmp; } /// \brief Divides a matrix by a scalar. @@ -1520,11 +1417,11 @@ mat operator*(mat&lhs, const mat& rhs) /// \param[in] rhs The scalar. /// \return The result. template -mat operator/(mat lhs, const S& rhs) +mat operator/(mat lhs, const S & rhs) { - lhs /= rhs; + lhs /= rhs; - return lhs; + return lhs; } /// \brief Adds two matrices together. @@ -1533,11 +1430,11 @@ mat operator/(mat lhs, const S& rhs) /// \param[in] rhs The right-side matrix. /// \return The resulting matrix. template -mat operator+(mat lhs, const mat& rhs) +mat operator+(mat lhs, const mat & rhs) { - lhs += rhs; + lhs += rhs; - return lhs; + return lhs; } /// \brief Subtracts a matrix from another matrix. @@ -1546,11 +1443,11 @@ mat operator+(mat lhs, const mat& rhs) /// \param[in] rhs The right-side matrix. /// \return The resulting matrix. template -mat operator-(mat lhs, const mat& rhs) +mat operator-(mat lhs, const mat & rhs) { - lhs -= rhs; + lhs -= rhs; - return lhs; + return lhs; } // Specific Typedefs ////////////////////////////////////////////////////////// @@ -1634,29 +1531,27 @@ typedef mat<4, 4, long double> mat44ld; /// /// \param[in] m The matrix to convert to string. /// \return The string representation. -template std::string str(mat m) +template +std::string str(mat m) { - std::stringstream ss; - ss << "["; + std::stringstream ss; + ss << "["; - for (size_t row_index = 0; row_index < m.dimRow(); row_index++) - { - ss << "("; + for (size_t row_index = 0; row_index < m.dimRow(); row_index++) { + ss << "("; - for (size_t col_index = 0; col_index < m.dimCol(); col_index++) - { - ss << m(row_index, col_index); + for (size_t col_index = 0; col_index < m.dimCol(); col_index++) { + ss << m(row_index, col_index); - if (col_index + 1 < m.dimCol()) - ss << "; "; - } + if (col_index + 1 < m.dimCol()) ss << "; "; + } - ss << ")"; - } + ss << ")"; + } - ss << "]"; + ss << "]"; - return ss.str(); + return ss.str(); } /// \brief Overloads the ostream << operator for easy usage in displaying @@ -1665,13 +1560,14 @@ template std::string str(mat std::ostream& operator<<(std::ostream& out, mat m) +template +std::ostream & operator<<(std::ostream & out, mat m) { - out << str(m); - return out; + out << str(m); + return out; } -} -} +} // namespace math +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/memoryport.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/memoryport.h index d51a9b36..b79d552a 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/memoryport.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/memoryport.h @@ -7,98 +7,96 @@ #define _MEMORYPORT_H_ #if defined(_MSC_VER) - #pragma warning(push) - #pragma warning(disable:4996) +#pragma warning(push) +#pragma warning(disable : 4996) #endif #include #if defined(_MSC_VER) - #pragma warning(pop) +#pragma warning(pop) #endif #include "int.h" -#include "port.h" #include "nocopy.h" +#include "port.h" -namespace vn { -namespace util { +namespace vn +{ +namespace util +{ /// \brief Useful test class for taking place where \ref vn::common::ISimplePort may be /// used. class MemoryPort : public xplat::IPort, private NoCopy { - typedef void(*DataWrittenHandler)(void* userData, const char* rawData, size_t length); + typedef void (*DataWrittenHandler)(void * userData, const char * rawData, size_t length); - // Constructors /////////////////////////////////////////////////////////// + // Constructors /////////////////////////////////////////////////////////// public: + /// \brief Creates a new \ref MemoryPort. + MemoryPort(); - /// \brief Creates a new \ref MemoryPort. - MemoryPort(); + ~MemoryPort(); - ~MemoryPort(); - - // Public Methods ///////////////////////////////////////////////////////// + // Public Methods ///////////////////////////////////////////////////////// public: + virtual void open(); - virtual void open(); - - virtual void close(); + virtual void close(); - virtual bool isOpen(); + virtual bool isOpen(); - virtual void write(const char data[], size_t length); + virtual void write(const char data[], size_t length); - virtual void read(char dataBuffer[], size_t numOfBytesToRead, size_t &numOfBytesActuallyRead); + virtual void read(char dataBuffer[], size_t numOfBytesToRead, size_t & numOfBytesActuallyRead); - virtual void registerDataReceivedHandler(void* userData, DataReceivedHandler handler); + virtual void registerDataReceivedHandler(void * userData, DataReceivedHandler handler); - virtual void unregisterDataReceivedHandler(); + virtual void unregisterDataReceivedHandler(); - /// \brief Registers a callback method for notification when new data is - /// written. - /// - /// \param[in] userData Pointer to user data, which will be provided to the - /// callback method. - /// \param[in] handler The callback method. - void registerDataWrittenHandler(void* userData, DataWrittenHandler handler); + /// \brief Registers a callback method for notification when new data is + /// written. + /// + /// \param[in] userData Pointer to user data, which will be provided to the + /// callback method. + /// \param[in] handler The callback method. + void registerDataWrittenHandler(void * userData, DataWrittenHandler handler); - /// \brief Unregisters the registered callback method. - void unregisterDataWrittenHandler(); + /// \brief Unregisters the registered callback method. + void unregisterDataWrittenHandler(); - /// \brief Sends data to the \ref MemoryPort which can then be read by - /// \ref read. - /// - /// \param[in] data Data buffer containing the data. - /// \param[in] length The number of data bytes. - void SendDataBackDoor(const uint8_t data[], size_t length); + /// \brief Sends data to the \ref MemoryPort which can then be read by + /// \ref read. + /// + /// \param[in] data Data buffer containing the data. + /// \param[in] length The number of data bytes. + void SendDataBackDoor(const uint8_t data[], size_t length); - /// \brief Sends data to the \ref MemoryPort which can then be read by - /// \ref read. - /// - /// \param[in] data Data buffer containing the data. - /// \param[in] length The number of data bytes. - void SendDataBackDoor(const char data[], size_t length); + /// \brief Sends data to the \ref MemoryPort which can then be read by + /// \ref read. + /// + /// \param[in] data Data buffer containing the data. + /// \param[in] length The number of data bytes. + void SendDataBackDoor(const char data[], size_t length); - /// \brief Sends data to the \ref MemoryPort which can then be read by - /// \ref read. - /// - /// \param[in] data The data to send. - void SendDataBackDoor(const std::string data); + /// \brief Sends data to the \ref MemoryPort which can then be read by + /// \ref read. + /// + /// \param[in] data The data to send. + void SendDataBackDoor(const std::string data); - // Private Members //////////////////////////////////////////////////////// + // Private Members //////////////////////////////////////////////////////// private: - - // Contains internal data, mainly stuff that is required for support. - struct Impl; - Impl *_pi; - + // Contains internal data, mainly stuff that is required for support. + struct Impl; + Impl * _pi; }; -} -} +} // namespace util +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/mock.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/mock.h index 2f93f603..620c8ca0 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/mock.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/mock.h @@ -1,10 +1,12 @@ #ifndef _VNSENSORS_MOCK_H_ #define _VNSENSORS_MOCK_H_ -namespace xplat { -namespace sensors { +namespace xplat +{ +namespace sensors +{ } -} +} // namespace xplat #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/nocopy.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/nocopy.h index 08f84da0..299e9850 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/nocopy.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/nocopy.h @@ -5,8 +5,10 @@ #include "export.h" -namespace vn { -namespace util { +namespace vn +{ +namespace util +{ /// \brief Identifies a derived class as being unable to be copied and prevents /// copy attempts. @@ -20,26 +22,22 @@ namespace util { /// \endcode class vn_proglib_DLLEXPORT NoCopy { - protected: + /// \brief Allows construction of derived objects. + NoCopy() {} - /// \brief Allows construction of derived objects. - NoCopy() { } - - /// \brief Allows destruction of derived objects. - ~NoCopy() { } + /// \brief Allows destruction of derived objects. + ~NoCopy() {} private: + /// \brief Prevent copying of derived objects. + NoCopy(const NoCopy &); - /// \brief Prevent copying of derived objects. - NoCopy(const NoCopy&); - - /// \brief Prevent assignment copying of derived objects. - NoCopy& operator=(const NoCopy&); - + /// \brief Prevent assignment copying of derived objects. + NoCopy & operator=(const NoCopy &); }; -} -} +} // namespace util +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/packet.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/packet.h index c8f9ceb3..7e9f11a9 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/packet.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/packet.h @@ -2,2213 +2,2459 @@ #define _VNPROTOCOL_UART_PACKET_H_ #include "int.h" -#include "vector.h" #include "matrix.h" #include "nocopy.h" #include "types.h" +#include "vector.h" -namespace vn { -namespace protocol { -namespace uart { +namespace vn +{ +namespace protocol +{ +namespace uart +{ /// \brief Structure representing a UART packet received from the VectorNav /// sensor. struct vn_proglib_DLLEXPORT Packet { - /// \brief Array containing sizes for the binary group fields. - static const unsigned char BinaryGroupLengths[sizeof(uint8_t)*8][sizeof(uint16_t)*15]; - - /// \brief The different types of UART packets. - enum Type - { - TYPE_UNKNOWN, ///< Type is unknown. - TYPE_BINARY, ///< Binary packet. - TYPE_ASCII ///< ASCII packet. - }; - - Packet(); - - /// \brief Creates a new packet based on the provided packet data buffer. A full - /// packet is expected which contains the deliminators (i.e. "$VNRRG,1*XX\r\n"). - /// - /// \param[in] packet Pointer to buffer containing the packet. - /// \param[in] length The number of bytes in the packet. - Packet(char const* packet, size_t length); - - explicit Packet(std::string packet); - - /// \brief Copy constructor. - /// - /// \param[in] toCopy The Packet to copy. - Packet(const Packet &toCopy); - - ~Packet(); - - /// \brief Assignment operator. - /// - /// \param[in] from The packet to assign from. - /// \return Reference to the newly copied packet. - Packet& operator=(const Packet &from); - - /// \brief Returns the encapsulated data as a string. - /// - /// \return The packet data. - std::string datastr(); - - /// \brief Returns the type of packet. - /// - /// \return The type of packet. - Type type(); - - /// \brief Performs data integrity check on the data packet. - /// - /// This will perform an 8-bit XOR checksum, a CRC16-CCITT CRC, or no - /// checking depending on the provided data integrity in the packet. - /// - /// \return true if the packet passed the data integrity checks; - /// otherwise false. - bool isValid(); - - /// \brief Indicates if the packet is an ASCII error message. - /// - /// \return true if the packet is an error message; otherwise - /// false. - bool isError(); - - /// \brief Indicates if the packet is a response to a message sent to the - /// sensor. - /// - /// \return true if the packet is a response message; otherwise - /// false. - bool isResponse(); - - /// \brief Indicates if the packet is an ASCII asynchronous message. - /// - /// \return true if the packet is an ASCII asynchronous message; - /// otherwise false. - bool isAsciiAsync(); - - /// \brief Indicates if the packet is a Bootloader message. - /// - /// \return true if the packet is a Bootloader message; - /// otherwise false. - bool isBootloader(); - - - /// \brief Determines the type of ASCII asynchronous message this packet - /// is. - /// - /// \return The asynchronous data type of the packet. - AsciiAsync determineAsciiAsyncType(); - - /// \brief Determines if the packet is a compatible match for an expected - /// binary output message type. - /// - /// \param[in] commonGroup The Common Group configuration. - /// \param[in] timeGroup The Time Group configuration. - /// \param[in] imuGroup The IMU Group configuration. - /// \param[in] gpsGroup The GPS Group configuration. - /// \param[in] attitudeGroup The Attitude Group configuration. - /// \param[in] insGroup The INS Group configuration. - /// \return true if the packet matches the expected group - /// configuration; otherwise false. - bool isCompatible(CommonGroup commonGroup, TimeGroup timeGroup, ImuGroup imuGroup, GpsGroup gpsGroup, AttitudeGroup attitudeGroup, InsGroup insGroup, GpsGroup gps2Group); - - /// \brief Computes the expected number of bytes for a possible binary - /// packet. - /// - /// This method requires that the group fields present and the complete - /// collection of individual group description fields are present. - /// - /// \param[in] startOfPossibleBinaryPacket The start of the possible binary - /// packet (i.e. the 0xFA character). - /// - /// \return The number of bytes expected for this binary packet. - static size_t computeBinaryPacketLength(const char *startOfPossibleBinaryPacket); - - /// \brief Computes the number of bytes expected for a binary group field. - /// - /// \param[in] group The group to calculate the total for. - /// \param[in] groupField The flags for data types present. - /// \return The number of bytes for this group. - static size_t computeNumOfBytesForBinaryGroupPayload(BinaryGroup group, uint16_t groupField); - - /// \brief Parses an error packet to get the error type. - /// - /// \return The sensor error. - SensorError parseError(); - - /// \brief If the packet is a binary message, this will return the groups field. - /// \return The present groups field. - uint8_t groups(); - - /// \brief This will return the requested group field of a binary packet at the - /// specified index. - /// - /// \param[in] index The 0-based index of the requested group field. - /// \return The group field. - uint16_t groupField(size_t index); - - /// \defgroup uartPacketBinaryExtractors UART Binary Data Extractors - /// \brief This group of methods are useful for extracting data from binary - /// data packets. - /// - /// \{ - - /// \brief Extracts a uint8_t data type from a binary packet and advances - /// the next extraction point appropriately. - /// - /// \return The extracted value. - uint8_t extractUint8(); - - /// \brief Extracts a int8_t data type from a binary packet and advances - /// the next extraction point appropriately. - /// - /// \return The extracted value. - int8_t extractInt8(); - - /// \brief Extracts a uint16_t data type from a binary packet and advances - /// the next extraction point appropriately. - /// - /// \return The extracted value. - uint16_t extractUint16(); - - /// \brief Extracts a uint32_t data type from a binary packet and advances - /// the next extraction point appropriately. - /// - /// \return The extracted value. - uint32_t extractUint32(); - - /// \brief Extracts a uint64_t data type from a binary packet and advances - /// the next extraction point appropriately. - /// - /// \return The extracted value. - uint64_t extractUint64(); - - /// \brief Extracts a float fdata type from a binary packet and advances - /// the next extraction point appropriately. - /// - /// \return The extracted value. - float extractFloat(); - - /// \brief Extracts a vec3f data type from a binary packet and advances - /// the next extraction point appropriately. - /// - /// \return The extracted value. - vn::math::vec3f extractVec3f(); - - /// \brief Extracts a vec3d data type from a binary packet and advances - /// the next extraction point appropriately. - /// - /// \return The extracted value. - vn::math::vec3d extractVec3d(); - - /// \brief Extracts a vec4f data type from a binary packet and advances - /// the next extraction point appropriately. - /// - /// \return The extracted value. - vn::math::vec4f extractVec4f(); - - /// \brief Extract a mat3f data type from a binary packet and advances - /// the next extraction point appropriately. - /// - /// \return The extracted value. - vn::math::mat3f extractMat3f(); - - /// \} - - /// \brief Appends astrick (*), checksum, and newlines to command. - /// - /// \param[in] errorDetectionMode The error detection type to append to the - /// command. - /// \param[in] packet The start of the packet. Should point to the '$' - /// character. - /// \param[in] length The current running length of the packet. - /// \return The final size of the command after appending the endings. - static size_t finalizeCommand(ErrorDetectionMode errorDetectionMode, char *packet, size_t length); - - /// \defgroup regReadWriteMethods Register Read/Write Generator Methods - /// \brief This group of methods will create commands that can be used to - /// read/write the register of a VectorNav sensor. - /// - /// \{ - - /// \brief Generates a command to read the Binary Output 1 register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadBinaryOutput1(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the Binary Output 2 register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadBinaryOutput2(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the Binary Output 13 register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadBinaryOutput3(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - - /// \brief Generates a command to write to the Binary Output 1 register on a VectorNav sensor. - /// - /// The field outputGroup available on the sensor's register is not - /// necessary as this method will compute the necessary value from the - /// provided data fields. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] asyncMode The register's asyncMode field. - /// \param[in] rateDivisor The register's rateDivisor field. - /// \param[in] commonField The register's Group 1 (Common) field. - /// \param[in] timeField The register's Group 2 (Time) field. - /// \param[in] imuField The register's Group 3 (IMU) field. - /// \param[in] gpsField The register's Group 4 (GPS) field. - /// \param[in] attitudeField The register's Group 5 (Attitude) field. - /// \param[in] insField The register's Group 6 (INS) field. + /// \brief Array containing sizes for the binary group fields. + static const unsigned char BinaryGroupLengths[sizeof(uint8_t) * 8][sizeof(uint16_t) * 15]; + + /// \brief The different types of UART packets. + enum Type { + TYPE_UNKNOWN, ///< Type is unknown. + TYPE_BINARY, ///< Binary packet. + TYPE_ASCII ///< ASCII packet. + }; + + Packet(); + + /// \brief Creates a new packet based on the provided packet data buffer. A full + /// packet is expected which contains the deliminators (i.e. "$VNRRG,1*XX\r\n"). + /// + /// \param[in] packet Pointer to buffer containing the packet. + /// \param[in] length The number of bytes in the packet. + Packet(char const * packet, size_t length); + + explicit Packet(std::string packet); + + /// \brief Copy constructor. + /// + /// \param[in] toCopy The Packet to copy. + Packet(const Packet & toCopy); + + ~Packet(); + + /// \brief Assignment operator. + /// + /// \param[in] from The packet to assign from. + /// \return Reference to the newly copied packet. + Packet & operator=(const Packet & from); + + /// \brief Returns the encapsulated data as a string. + /// + /// \return The packet data. + std::string datastr(); + + /// \brief Returns the type of packet. + /// + /// \return The type of packet. + Type type(); + + /// \brief Performs data integrity check on the data packet. + /// + /// This will perform an 8-bit XOR checksum, a CRC16-CCITT CRC, or no + /// checking depending on the provided data integrity in the packet. + /// + /// \return true if the packet passed the data integrity checks; + /// otherwise false. + bool isValid(); + + /// \brief Indicates if the packet is an ASCII error message. + /// + /// \return true if the packet is an error message; otherwise + /// false. + bool isError(); + + /// \brief Indicates if the packet is a response to a message sent to the + /// sensor. + /// + /// \return true if the packet is a response message; otherwise + /// false. + bool isResponse(); + + /// \brief Indicates if the packet is an ASCII asynchronous message. + /// + /// \return true if the packet is an ASCII asynchronous message; + /// otherwise false. + bool isAsciiAsync(); + + /// \brief Indicates if the packet is a Bootloader message. + /// + /// \return true if the packet is a Bootloader message; + /// otherwise false. + bool isBootloader(); + + /// \brief Determines the type of ASCII asynchronous message this packet + /// is. + /// + /// \return The asynchronous data type of the packet. + AsciiAsync determineAsciiAsyncType(); + + /// \brief Determines if the packet is a compatible match for an expected + /// binary output message type. + /// + /// \param[in] commonGroup The Common Group configuration. + /// \param[in] timeGroup The Time Group configuration. + /// \param[in] imuGroup The IMU Group configuration. + /// \param[in] gpsGroup The GPS Group configuration. + /// \param[in] attitudeGroup The Attitude Group configuration. + /// \param[in] insGroup The INS Group configuration. + /// \return true if the packet matches the expected group + /// configuration; otherwise false. + bool isCompatible( + CommonGroup commonGroup, TimeGroup timeGroup, ImuGroup imuGroup, GpsGroup gpsGroup, + AttitudeGroup attitudeGroup, InsGroup insGroup, GpsGroup gps2Group); + + /// \brief Computes the expected number of bytes for a possible binary + /// packet. + /// + /// This method requires that the group fields present and the complete + /// collection of individual group description fields are present. + /// + /// \param[in] startOfPossibleBinaryPacket The start of the possible binary + /// packet (i.e. the 0xFA character). + /// + /// \return The number of bytes expected for this binary packet. + static size_t computeBinaryPacketLength(const char * startOfPossibleBinaryPacket); + + /// \brief Computes the number of bytes expected for a binary group field. + /// + /// \param[in] group The group to calculate the total for. + /// \param[in] groupField The flags for data types present. + /// \return The number of bytes for this group. + static size_t computeNumOfBytesForBinaryGroupPayload(BinaryGroup group, uint16_t groupField); + + /// \brief Parses an error packet to get the error type. + /// + /// \return The sensor error. + SensorError parseError(); + + /// \brief If the packet is a binary message, this will return the groups field. + /// \return The present groups field. + uint8_t groups(); + + /// \brief This will return the requested group field of a binary packet at the + /// specified index. + /// + /// \param[in] index The 0-based index of the requested group field. + /// \return The group field. + uint16_t groupField(size_t index); + + /// \defgroup uartPacketBinaryExtractors UART Binary Data Extractors + /// \brief This group of methods are useful for extracting data from binary + /// data packets. + /// + /// \{ + + /// \brief Extracts a uint8_t data type from a binary packet and advances + /// the next extraction point appropriately. + /// + /// \return The extracted value. + uint8_t extractUint8(); + + /// \brief Extracts a int8_t data type from a binary packet and advances + /// the next extraction point appropriately. + /// + /// \return The extracted value. + int8_t extractInt8(); + + /// \brief Extracts a uint16_t data type from a binary packet and advances + /// the next extraction point appropriately. + /// + /// \return The extracted value. + uint16_t extractUint16(); + + /// \brief Extracts a uint32_t data type from a binary packet and advances + /// the next extraction point appropriately. + /// + /// \return The extracted value. + uint32_t extractUint32(); + + /// \brief Extracts a uint64_t data type from a binary packet and advances + /// the next extraction point appropriately. + /// + /// \return The extracted value. + uint64_t extractUint64(); + + /// \brief Extracts a float fdata type from a binary packet and advances + /// the next extraction point appropriately. + /// + /// \return The extracted value. + float extractFloat(); + + /// \brief Extracts a vec3f data type from a binary packet and advances + /// the next extraction point appropriately. + /// + /// \return The extracted value. + vn::math::vec3f extractVec3f(); + + /// \brief Extracts a vec3d data type from a binary packet and advances + /// the next extraction point appropriately. + /// + /// \return The extracted value. + vn::math::vec3d extractVec3d(); + + /// \brief Extracts a vec4f data type from a binary packet and advances + /// the next extraction point appropriately. + /// + /// \return The extracted value. + vn::math::vec4f extractVec4f(); + + /// \brief Extract a mat3f data type from a binary packet and advances + /// the next extraction point appropriately. + /// + /// \return The extracted value. + vn::math::mat3f extractMat3f(); + + /// \} + + /// \brief Appends astrick (*), checksum, and newlines to command. + /// + /// \param[in] errorDetectionMode The error detection type to append to the + /// command. + /// \param[in] packet The start of the packet. Should point to the '$' + /// character. + /// \param[in] length The current running length of the packet. + /// \return The final size of the command after appending the endings. + static size_t finalizeCommand( + ErrorDetectionMode errorDetectionMode, char * packet, size_t length); + + /// \defgroup regReadWriteMethods Register Read/Write Generator Methods + /// \brief This group of methods will create commands that can be used to + /// read/write the register of a VectorNav sensor. + /// + /// \{ + + /// \brief Generates a command to read the Binary Output 1 register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadBinaryOutput1( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the Binary Output 2 register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadBinaryOutput2( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the Binary Output 13 register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadBinaryOutput3( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Binary Output 1 register on a VectorNav sensor. + /// + /// The field outputGroup available on the sensor's register is not + /// necessary as this method will compute the necessary value from the + /// provided data fields. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] asyncMode The register's asyncMode field. + /// \param[in] rateDivisor The register's rateDivisor field. + /// \param[in] commonField The register's Group 1 (Common) field. + /// \param[in] timeField The register's Group 2 (Time) field. + /// \param[in] imuField The register's Group 3 (IMU) field. + /// \param[in] gpsField The register's Group 4 (GPS) field. + /// \param[in] attitudeField The register's Group 5 (Attitude) field. + /// \param[in] insField The register's Group 6 (INS) field. /// \param[in] gps2Field The register's Group 7 (GPS2) field. -/// \return The total number bytes in the generated command. - static size_t genWriteBinaryOutput1(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint16_t asyncMode, uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, uint16_t imuField, uint16_t gpsField, uint16_t attitudeField, uint16_t insField, uint16_t gps2Field); - - /// \brief Generates a command to write to the Binary Output 2 register on a VectorNav sensor. - /// - /// The field outputGroup available on the sensor's register is not - /// necessary as this method will compute the necessary value from the - /// provided data fields. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] asyncMode The register's asyncMode field. - /// \param[in] rateDivisor The register's rateDivisor field. - /// \param[in] commonField The register's Group 1 (Common) field. - /// \param[in] timeField The register's Group 2 (Time) field. - /// \param[in] imuField The register's Group 3 (IMU) field. - /// \param[in] gpsField The register's Group 4 (GPS) field. - /// \param[in] attitudeField The register's Group 5 (Attitude) field. - /// \param[in] insField The register's Group 6 (INS) field. + /// \return The total number bytes in the generated command. + static size_t genWriteBinaryOutput1( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint16_t asyncMode, + uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, uint16_t imuField, + uint16_t gpsField, uint16_t attitudeField, uint16_t insField, uint16_t gps2Field); + + /// \brief Generates a command to write to the Binary Output 2 register on a VectorNav sensor. + /// + /// The field outputGroup available on the sensor's register is not + /// necessary as this method will compute the necessary value from the + /// provided data fields. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] asyncMode The register's asyncMode field. + /// \param[in] rateDivisor The register's rateDivisor field. + /// \param[in] commonField The register's Group 1 (Common) field. + /// \param[in] timeField The register's Group 2 (Time) field. + /// \param[in] imuField The register's Group 3 (IMU) field. + /// \param[in] gpsField The register's Group 4 (GPS) field. + /// \param[in] attitudeField The register's Group 5 (Attitude) field. + /// \param[in] insField The register's Group 6 (INS) field. /// \param[in] gps2Field The register's Group 7 (GPS2) field. /// \return The total number bytes in the generated command. - static size_t genWriteBinaryOutput2(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint16_t asyncMode, uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, uint16_t imuField, uint16_t gpsField, uint16_t attitudeField, uint16_t insField, uint16_t gps2Field); - - /// \brief Generates a command to write to the Binary Output 3 register on a VectorNav sensor. - /// - /// The field outputGroup available on the sensor's register is not - /// necessary as this method will compute the necessary value from the - /// provided data fields. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] asyncMode The register's asyncMode field. - /// \param[in] rateDivisor The register's rateDivisor field. - /// \param[in] commonField The register's Group 1 (Common) field. - /// \param[in] timeField The register's Group 2 (Time) field. - /// \param[in] imuField The register's Group 3 (IMU) field. - /// \param[in] gpsField The register's Group 4 (GPS) field. - /// \param[in] attitudeField The register's Group 5 (Attitude) field. - /// \param[in] insField The register's Group 6 (INS) field. + static size_t genWriteBinaryOutput2( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint16_t asyncMode, + uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, uint16_t imuField, + uint16_t gpsField, uint16_t attitudeField, uint16_t insField, uint16_t gps2Field); + + /// \brief Generates a command to write to the Binary Output 3 register on a VectorNav sensor. + /// + /// The field outputGroup available on the sensor's register is not + /// necessary as this method will compute the necessary value from the + /// provided data fields. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] asyncMode The register's asyncMode field. + /// \param[in] rateDivisor The register's rateDivisor field. + /// \param[in] commonField The register's Group 1 (Common) field. + /// \param[in] timeField The register's Group 2 (Time) field. + /// \param[in] imuField The register's Group 3 (IMU) field. + /// \param[in] gpsField The register's Group 4 (GPS) field. + /// \param[in] attitudeField The register's Group 5 (Attitude) field. + /// \param[in] insField The register's Group 6 (INS) field. /// \param[in] gps2Field The register's Group 7 (GPS2) field. /// \return The total number bytes in the generated command. - static size_t genWriteBinaryOutput3(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint16_t asyncMode, uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, uint16_t imuField, uint16_t gpsField, uint16_t attitudeField, uint16_t insField, uint16_t gps2Field); - - - /// \brief Generates a command to write sensor settings to non-volatile memory. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genWriteSettings(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size); - - /// \brief Generates a command to tare the sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genTare(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size); - - /// \brief Generates a command to alert the sensor of a known magnetic disturbance. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] indicates if a known magnetic disturbance is present or not. - /// \return The total number bytes in the generated command. - static size_t genKnownMagneticDisturbance(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, bool isMagneticDisturbancePresent); - - /// \brief Generates a command to alert the sensor of a known acceleration disturbance. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] indicates if a known acceleration disturbance is present or not. - /// \return The total number bytes in the generated command. - static size_t genKnownAccelerationDisturbance(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, bool isAccelerationDisturbancePresent); - - /// \brief Generates a command to set the gyro bias. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genSetGyroBias(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size); - - /// \brief Generates a command to retore factory settings. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genRestoreFactorySettings(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size); - - /// \brief Generates a command to reset the sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReset(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size); - - /// \brief Generates a command to put the sensor in firmware update mode. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genFirmwareUpdate(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the Serial Baud Rate register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] port The port to read from. - /// \return The total number bytes in the generated command. - static size_t genReadSerialBaudRate(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t port); - - /// \brief Generates a command to write to the Serial Baud Rate register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] baudrate The register's Baud Rate field. - /// \param[in] port The port to write to. - /// \return The total number bytes in the generated command. - static size_t genWriteSerialBaudRate(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint32_t baudrate, uint8_t port); - - /// \brief Generates a command to read the Async Data Output Type register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] port The port to read from. - /// \return The total number bytes in the generated command. - static size_t genReadAsyncDataOutputType(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t port); - - /// \brief Generates a command to write to the Async Data Output Type register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] ador The register's ADOR field. - /// \param[in] port The port to write to. - /// \return The total number bytes in the generated command. - static size_t genWriteAsyncDataOutputType(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint32_t ador, uint8_t port); - - /// \brief Generates a command to read the Async Data Output Frequency register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] port The port to read from. - /// \return The total number bytes in the generated command. - static size_t genReadAsyncDataOutputFrequency(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t port); - - /// \brief Generates a command to write to the Async Data Output Frequency register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] adof The register's ADOF field. - /// \param[in] port The port to write to. - /// \return The total number bytes in the generated command. - static size_t genWriteAsyncDataOutputFrequency(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint32_t adof, uint8_t port); - - /// \brief Generates a command to write to a firmware update record on a VectorNav sensor to the bootloader. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] record The record to write to the bootloader for a specific processor on the sensor. - /// \return The total number bytes in the generated command. - static size_t genWriteFirmwareUpdateRecord(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, std::string record); - - /// \brief Generates a command to read the User Tag register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadUserTag(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the User Tag register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] tag The register's Tag field. - /// \return The total number bytes in the generated command. - static size_t genWriteUserTag(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, std::string tag); - - /// \brief Generates a command to read the Model Number register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadModelNumber(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the Hardware Revision register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadHardwareRevision(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the Serial Number register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadSerialNumber(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the Firmware Version register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadFirmwareVersion(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the Serial Baud Rate register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadSerialBaudRate(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Serial Baud Rate register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] baudrate The register's Baud Rate field. - /// \return The total number bytes in the generated command. - static size_t genWriteSerialBaudRate(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint32_t baudrate); - - /// \brief Generates a command to read the Async Data Output Type register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadAsyncDataOutputType(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Async Data Output Type register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] ador The register's ADOR field. - /// \return The total number bytes in the generated command. - static size_t genWriteAsyncDataOutputType(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint32_t ador); - - /// \brief Generates a command to read the Async Data Output Frequency register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadAsyncDataOutputFrequency(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Async Data Output Frequency register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] adof The register's ADOF field. - /// \return The total number bytes in the generated command. - static size_t genWriteAsyncDataOutputFrequency(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint32_t adof); - - /// \brief Generates a command to read the Yaw Pitch Roll register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadYawPitchRoll(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the Attitude Quaternion register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadAttitudeQuaternion(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the Quaternion, Magnetic, Acceleration and Angular Rates register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadQuaternionMagneticAccelerationAndAngularRates(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the Magnetic Measurements register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadMagneticMeasurements(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the Acceleration Measurements register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadAccelerationMeasurements(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the Angular Rate Measurements register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadAngularRateMeasurements(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the Magnetic, Acceleration and Angular Rates register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadMagneticAccelerationAndAngularRates(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the Magnetic and Gravity Reference Vectors register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadMagneticAndGravityReferenceVectors(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Magnetic and Gravity Reference Vectors register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] magRef The register's MagRef field. - /// \param[in] accRef The register's AccRef field. - /// \return The total number bytes in the generated command. - static size_t genWriteMagneticAndGravityReferenceVectors(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, vn::math::vec3f magRef, vn::math::vec3f accRef); - - /// \brief Generates a command to read the Filter Measurements Variance Parameters register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadFilterMeasurementsVarianceParameters(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Filter Measurements Variance Parameters register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] angularWalkVariance The register's Angular Walk Variance field. - /// \param[in] angularRateVariance The register's Angular Rate Variance field. - /// \param[in] magneticVariance The register's Magnetic Variance field. - /// \param[in] accelerationVariance The register's Acceleration Variance field. - /// \return The total number bytes in the generated command. - static size_t genWriteFilterMeasurementsVarianceParameters(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, float angularWalkVariance, vn::math::vec3f angularRateVariance, vn::math::vec3f magneticVariance, vn::math::vec3f accelerationVariance); - - /// \brief Generates a command to read the Magnetometer Compensation register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadMagnetometerCompensation(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Magnetometer Compensation register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] c The register's C field. - /// \param[in] b The register's B field. - /// \return The total number bytes in the generated command. - static size_t genWriteMagnetometerCompensation(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, vn::math::mat3f c, vn::math::vec3f b); - - /// \brief Generates a command to read the Filter Active Tuning Parameters register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadFilterActiveTuningParameters(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Filter Active Tuning Parameters register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] magneticDisturbanceGain The register's Magnetic Disturbance Gain field. - /// \param[in] accelerationDisturbanceGain The register's Acceleration Disturbance Gain field. - /// \param[in] magneticDisturbanceMemory The register's Magnetic Disturbance Memory field. - /// \param[in] accelerationDisturbanceMemory The register's Acceleration Disturbance Memory field. - /// \return The total number bytes in the generated command. - static size_t genWriteFilterActiveTuningParameters(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, float magneticDisturbanceGain, float accelerationDisturbanceGain, float magneticDisturbanceMemory, float accelerationDisturbanceMemory); - - /// \brief Generates a command to read the Acceleration Compensation register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadAccelerationCompensation(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Acceleration Compensation register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] c The register's C field. - /// \param[in] b The register's B field. - /// \return The total number bytes in the generated command. - static size_t genWriteAccelerationCompensation(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, vn::math::mat3f c, vn::math::vec3f b); - - /// \brief Generates a command to read the Reference Frame Rotation register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadReferenceFrameRotation(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Reference Frame Rotation register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] c The register's C field. - /// \return The total number bytes in the generated command. - static size_t genWriteReferenceFrameRotation(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, vn::math::mat3f c); - - /// \brief Generates a command to read the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadYawPitchRollMagneticAccelerationAndAngularRates(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the Communication Protocol Control register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadCommunicationProtocolControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Communication Protocol Control register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] serialCount The register's SerialCount field. - /// \param[in] serialStatus The register's SerialStatus field. - /// \param[in] spiCount The register's SPICount field. - /// \param[in] spiStatus The register's SPIStatus field. - /// \param[in] serialChecksum The register's SerialChecksum field. - /// \param[in] spiChecksum The register's SPIChecksum field. - /// \param[in] errorMode The register's ErrorMode field. - /// \return The total number bytes in the generated command. - static size_t genWriteCommunicationProtocolControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t serialCount, uint8_t serialStatus, uint8_t spiCount, uint8_t spiStatus, uint8_t serialChecksum, uint8_t spiChecksum, uint8_t errorMode); - - /// \brief Generates a command to read the Synchronization Control register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadSynchronizationControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Synchronization Control register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] syncInMode The register's SyncInMode field. - /// \param[in] syncInEdge The register's SyncInEdge field. - /// \param[in] syncInSkipFactor The register's SyncInSkipFactor field. - /// \param[in] syncOutMode The register's SyncOutMode field. - /// \param[in] syncOutPolarity The register's SyncOutPolarity field. - /// \param[in] syncOutSkipFactor The register's SyncOutSkipFactor field. - /// \param[in] syncOutPulseWidth The register's SyncOutPulseWidth field. - /// \return The total number bytes in the generated command. - static size_t genWriteSynchronizationControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t syncInMode, uint8_t syncInEdge, uint16_t syncInSkipFactor, uint8_t syncOutMode, uint8_t syncOutPolarity, uint16_t syncOutSkipFactor, uint32_t syncOutPulseWidth); - - /// \brief Generates a command to read the Synchronization Status register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadSynchronizationStatus(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Synchronization Status register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] syncInCount The register's SyncInCount field. - /// \param[in] syncInTime The register's SyncInTime field. - /// \param[in] syncOutCount The register's SyncOutCount field. - /// \return The total number bytes in the generated command. - static size_t genWriteSynchronizationStatus(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint32_t syncInCount, uint32_t syncInTime, uint32_t syncOutCount); - - /// \brief Generates a command to read the Filter Basic Control register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadFilterBasicControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Filter Basic Control register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] magMode The register's MagMode field. - /// \param[in] extMagMode The register's ExtMagMode field. - /// \param[in] extAccMode The register's ExtAccMode field. - /// \param[in] extGyroMode The register's ExtGyroMode field. - /// \param[in] gyroLimit The register's GyroLimit field. - /// \return The total number bytes in the generated command. - static size_t genWriteFilterBasicControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t magMode, uint8_t extMagMode, uint8_t extAccMode, uint8_t extGyroMode, vn::math::vec3f gyroLimit); - - /// \brief Generates a command to read the Heave Configuration register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadHeaveConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size); - - /// \brief Generates a command to write to the VPE Basic Control register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] initialWavePeriod Value for the initialWavePeriod field. - /// \param[in] initialWaveAmplitude Value for the initialWaveAmplitude field. - /// \param[in] maxWavePeriod Value for the maxWavePeriod field. - /// \param[in] minWaveAmplitude Value for the minWaveAmplitude field. - /// \param[in] delayedHeaveCutoffFreq Value for the delayedHeaveCutoffFreq field. - /// \param[in] heaveCutoffFreq Value for the heaveCutoffFreq field. - /// \param[in] heaveRateCutoffFreq Value for the heaveRateCutoffFreq field. - /// \return The total number bytes in the generated command. - static size_t genWriteHeaveConfiguration(ErrorDetectionMode errorDetectionMode,char *buffer, size_t size, - float initialWavePeriod, - float initialWaveAmplitude, - float maxWavePeriod, - float minWaveAmplitude, - float delayedHeaveCutoffFreq, - float heaveCutoffFreq, - float heaveRateCutoffFreq); - - /// \brief Generates a command to read the VPE Basic Control register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadVpeBasicControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the VPE Basic Control register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] enable The register's Enable field. - /// \param[in] headingMode The register's HeadingMode field. - /// \param[in] filteringMode The register's FilteringMode field. - /// \param[in] tuningMode The register's TuningMode field. - /// \return The total number bytes in the generated command. - static size_t genWriteVpeBasicControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t enable, uint8_t headingMode, uint8_t filteringMode, uint8_t tuningMode); - - /// \brief Generates a command to read the VPE Magnetometer Basic Tuning register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadVpeMagnetometerBasicTuning(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the VPE Magnetometer Basic Tuning register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] baseTuning The register's BaseTuning field. - /// \param[in] adaptiveTuning The register's AdaptiveTuning field. - /// \param[in] adaptiveFiltering The register's AdaptiveFiltering field. - /// \return The total number bytes in the generated command. - static size_t genWriteVpeMagnetometerBasicTuning(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, vn::math::vec3f baseTuning, vn::math::vec3f adaptiveTuning, vn::math::vec3f adaptiveFiltering); - - /// \brief Generates a command to read the VPE Magnetometer Advanced Tuning register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadVpeMagnetometerAdvancedTuning(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the VPE Magnetometer Advanced Tuning register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] minFiltering The register's MinFiltering field. - /// \param[in] maxFiltering The register's MaxFiltering field. - /// \param[in] maxAdaptRate The register's MaxAdaptRate field. - /// \param[in] disturbanceWindow The register's DisturbanceWindow field. - /// \param[in] maxTuning The register's MaxTuning field. - /// \return The total number bytes in the generated command. - static size_t genWriteVpeMagnetometerAdvancedTuning(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, vn::math::vec3f minFiltering, vn::math::vec3f maxFiltering, float maxAdaptRate, float disturbanceWindow, float maxTuning); - - /// \brief Generates a command to read the VPE Accelerometer Basic Tuning register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadVpeAccelerometerBasicTuning(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the VPE Accelerometer Basic Tuning register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] baseTuning The register's BaseTuning field. - /// \param[in] adaptiveTuning The register's AdaptiveTuning field. - /// \param[in] adaptiveFiltering The register's AdaptiveFiltering field. - /// \return The total number bytes in the generated command. - static size_t genWriteVpeAccelerometerBasicTuning(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, vn::math::vec3f baseTuning, vn::math::vec3f adaptiveTuning, vn::math::vec3f adaptiveFiltering); - - /// \brief Generates a command to read the VPE Accelerometer Advanced Tuning register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadVpeAccelerometerAdvancedTuning(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the VPE Accelerometer Advanced Tuning register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] minFiltering The register's MinFiltering field. - /// \param[in] maxFiltering The register's MaxFiltering field. - /// \param[in] maxAdaptRate The register's MaxAdaptRate field. - /// \param[in] disturbanceWindow The register's DisturbanceWindow field. - /// \param[in] maxTuning The register's MaxTuning field. - /// \return The total number bytes in the generated command. - static size_t genWriteVpeAccelerometerAdvancedTuning(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, vn::math::vec3f minFiltering, vn::math::vec3f maxFiltering, float maxAdaptRate, float disturbanceWindow, float maxTuning); - - /// \brief Generates a command to read the VPE Gyro Basic Tuning register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadVpeGyroBasicTuning(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the VPE Gyro Basic Tuning register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] angularWalkVariance The register's AngularWalkVariance field. - /// \param[in] baseTuning The register's BaseTuning field. - /// \param[in] adaptiveTuning The register's AdaptiveTuning field. - /// \return The total number bytes in the generated command. - static size_t genWriteVpeGyroBasicTuning(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, vn::math::vec3f angularWalkVariance, vn::math::vec3f baseTuning, vn::math::vec3f adaptiveTuning); - - /// \brief Generates a command to read the Filter Startup Gyro Bias register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadFilterStartupGyroBias(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Filter Startup Gyro Bias register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] bias The register's Bias field. - /// \return The total number bytes in the generated command. - static size_t genWriteFilterStartupGyroBias(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, vn::math::vec3f bias); - - /// \brief Generates a command to read the Magnetometer Calibration Control register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadMagnetometerCalibrationControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Magnetometer Calibration Control register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] hsiMode The register's HSIMode field. - /// \param[in] hsiOutput The register's HSIOutput field. - /// \param[in] convergeRate The register's ConvergeRate field. - /// \return The total number bytes in the generated command. - static size_t genWriteMagnetometerCalibrationControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t hsiMode, uint8_t hsiOutput, uint8_t convergeRate); - - /// \brief Generates a command to read the Calculated Magnetometer Calibration register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadCalculatedMagnetometerCalibration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the Indoor Heading Mode Control register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadIndoorHeadingModeControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Indoor Heading Mode Control register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] maxRateError The register's Max Rate Error field. - /// \return The total number bytes in the generated command. - static size_t genWriteIndoorHeadingModeControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, float maxRateError); - - /// \brief Generates a command to read the Velocity Compensation Measurement register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadVelocityCompensationMeasurement(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Velocity Compensation Measurement register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] velocity The register's Velocity field. - /// \return The total number bytes in the generated command. - static size_t genWriteVelocityCompensationMeasurement(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, vn::math::vec3f velocity); - - /// \brief Generates a command to read the Velocity Compensation Control register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadVelocityCompensationControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Velocity Compensation Control register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] mode The register's Mode field. - /// \param[in] velocityTuning The register's VelocityTuning field. - /// \param[in] rateTuning The register's RateTuning field. - /// \return The total number bytes in the generated command. - static size_t genWriteVelocityCompensationControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t mode, float velocityTuning, float rateTuning); - - /// \brief Generates a command to read the Velocity Compensation Status register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadVelocityCompensationStatus(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the IMU Measurements register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadImuMeasurements(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the GPS Configuration register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadGpsConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the GPS Configuration register on a VectorNav sensor (deprecated). - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] mode The register's Mode field. - /// \param[in] ppsSource The register's PpsSource field. - /// \return The total number bytes in the generated command. - static size_t genWriteGpsConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t mode, uint8_t ppsSource); - - /// \brief Generates a command to write to the GPS Configuration register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] mode The register's Mode field. - /// \param[in] ppsSource The register's PpsSource field. - /// \param[in] rate The register's Rate field. - /// \param[in] antPow The register's AntPower field. - /// \return The total number bytes in the generated command. - static size_t genWriteGpsConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t mode, uint8_t ppsSource, uint8_t rate, uint8_t antPow); - - /// \brief Generates a command to read the GPS Antenna Offset register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadGpsAntennaOffset(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the GPS Antenna Offset register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] position The register's Position field. - /// \return The total number bytes in the generated command. - static size_t genWriteGpsAntennaOffset(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, vn::math::vec3f position); - - /// \brief Generates a command to read the GPS Solution - LLA register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadGpsSolutionLla(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the GPS Solution - ECEF register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadGpsSolutionEcef(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the INS Solution - LLA register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadInsSolutionLla(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the INS Solution - ECEF register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadInsSolutionEcef(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the INS Basic Configuration register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadInsBasicConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the INS Basic Configuration register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] scenario The register's Scenario field. - /// \param[in] ahrsAiding The register's AhrsAiding field. - /// \param[in] estBaseline The register's EstBaseline field. - /// \return The total number bytes in the generated command. - static size_t genWriteInsBasicConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t scenario, uint8_t ahrsAiding, uint8_t estBaseline); - - /// \brief Generates a command to read the INS Advanced Configuration register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadInsAdvancedConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the INS Advanced Configuration register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] useMag The register's UseMag field. - /// \param[in] usePres The register's UsePres field. - /// \param[in] posAtt The register's PosAtt field. - /// \param[in] velAtt The register's VelAtt field. - /// \param[in] velBias The register's VelBias field. - /// \param[in] useFoam The register's UseFoam field. - /// \param[in] gpsCovType The register's GPSCovType field. - /// \param[in] velCount The register's VelCount field. - /// \param[in] velInit The register's VelInit field. - /// \param[in] moveOrigin The register's MoveOrigin field. - /// \param[in] gpsTimeout The register's GPSTimeout field. - /// \param[in] deltaLimitPos The register's DeltaLimitPos field. - /// \param[in] deltaLimitVel The register's DeltaLimitVel field. - /// \param[in] minPosUncertainty The register's MinPosUncertainty field. - /// \param[in] minVelUncertainty The register's MinVelUncertainty field. - /// \return The total number bytes in the generated command. - static size_t genWriteInsAdvancedConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t useMag, uint8_t usePres, uint8_t posAtt, uint8_t velAtt, uint8_t velBias, uint8_t useFoam, uint8_t gpsCovType, uint8_t velCount, float velInit, float moveOrigin, float gpsTimeout, float deltaLimitPos, float deltaLimitVel, float minPosUncertainty, float minVelUncertainty); - - /// \brief Generates a command to read the INS State - LLA register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadInsStateLla(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the INS State - ECEF register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadInsStateEcef(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the Startup Filter Bias Estimate register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadStartupFilterBiasEstimate(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Startup Filter Bias Estimate register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] gyroBias The register's GyroBias field. - /// \param[in] accelBias The register's AccelBias field. - /// \param[in] pressureBias The register's PressureBias field. - /// \return The total number bytes in the generated command. - static size_t genWriteStartupFilterBiasEstimate(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, vn::math::vec3f gyroBias, vn::math::vec3f accelBias, float pressureBias); - - /// \brief Generates a command to read the Delta Theta and Delta Velocity register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadDeltaThetaAndDeltaVelocity(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the Delta Theta and Delta Velocity Configuration register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadDeltaThetaAndDeltaVelocityConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Delta Theta and Delta Velocity Configuration register on a VectorNav sensor (deprecated). - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] integrationFrame The register's IntegrationFrame field. - /// \param[in] gyroCompensation The register's GyroCompensation field. - /// \param[in] accelCompensation The register's AccelCompensation field. - /// \return The total number bytes in the generated command. - static size_t genWriteDeltaThetaAndDeltaVelocityConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t integrationFrame, uint8_t gyroCompensation, uint8_t accelCompensation); - - /// \brief Generates a command to write to the Delta Theta and Delta Velocity Configuration register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] integrationFrame The register's IntegrationFrame field. - /// \param[in] gyroCompensation The register's GyroCompensation field. - /// \param[in] accelCompensation The register's AccelCompensation field. - /// \param[in] earthRateCorrection The register's EarthRateCorrection field. - /// \return The total number bytes in the generated command. - static size_t genWriteDeltaThetaAndDeltaVelocityConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t integrationFrame, uint8_t gyroCompensation, uint8_t accelCompensation, uint8_t earthRateCorrection); - - /// \brief Generates a command to read the Reference Vector Configuration register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadReferenceVectorConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Reference Vector Configuration register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] useMagModel The register's UseMagModel field. - /// \param[in] useGravityModel The register's UseGravityModel field. - /// \param[in] recalcThreshold The register's RecalcThreshold field. - /// \param[in] year The register's Year field. - /// \param[in] position The register's Position field. - /// \return The total number bytes in the generated command. - static size_t genWriteReferenceVectorConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t useMagModel, uint8_t useGravityModel, uint32_t recalcThreshold, float year, vn::math::vec3d position); - - /// \brief Generates a command to read the Gyro Compensation register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadGyroCompensation(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the Gyro Compensation register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] c The register's C field. - /// \param[in] b The register's B field. - /// \return The total number bytes in the generated command. - static size_t genWriteGyroCompensation(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, vn::math::mat3f c, vn::math::vec3f b); - - /// \brief Generates a command to read the IMU Filtering Configuration register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadImuFilteringConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the IMU Filtering Configuration register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] magWindowSize The register's MagWindowSize field. - /// \param[in] accelWindowSize The register's AccelWindowSize field. - /// \param[in] gyroWindowSize The register's GyroWindowSize field. - /// \param[in] tempWindowSize The register's TempWindowSize field. - /// \param[in] presWindowSize The register's PresWindowSize field. - /// \param[in] magFilterMode The register's MagFilterMode field. - /// \param[in] accelFilterMode The register's AccelFilterMode field. - /// \param[in] gyroFilterMode The register's GyroFilterMode field. - /// \param[in] tempFilterMode The register's TempFilterMode field. - /// \param[in] presFilterMode The register's PresFilterMode field. - /// \return The total number bytes in the generated command. - static size_t genWriteImuFilteringConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, 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 Generates a command to read the GPS Compass Baseline register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadGpsCompassBaseline(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the GPS Compass Baseline register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] position The register's Position field. - /// \param[in] uncertainty The register's Uncertainty field. - /// \return The total number bytes in the generated command. - static size_t genWriteGpsCompassBaseline(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, vn::math::vec3f position, vn::math::vec3f uncertainty); - - /// \brief Generates a command to read the GPS Compass Estimated Baseline register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadGpsCompassEstimatedBaseline(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the IMU Rate Configuration register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadImuRateConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to write to the IMU Rate Configuration register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \param[in] imuRate The register's imuRate field. - /// \param[in] navDivisor The register's NavDivisor field. - /// \param[in] filterTargetRate The register's filterTargetRate field. - /// \param[in] filterMinRate The register's filterMinRate field. - /// \return The total number bytes in the generated command. - static size_t genWriteImuRateConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint16_t imuRate, uint16_t navDivisor, float filterTargetRate, float filterMinRate); - - /// \brief Generates a command to read the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadYawPitchRollTrueBodyAccelerationAndAngularRates(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \brief Generates a command to read the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register on a VectorNav sensor. - /// - /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. - /// \param[in] buffer Caller provided buffer to place the generated command. - /// \param[in] size Number of bytes available in the provided buffer. - /// \return The total number bytes in the generated command. - static size_t genReadYawPitchRollTrueInertialAccelerationAndAngularRates(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); - - /// \} - - /// \defgroup uartPacketAsciiAsyncParsers UART ASCII Asynchronous Packet Parsers - /// \brief This group of methods allow parsing of ASCII asynchronous data - /// packets from VectorNav sensors. - /// - /// The units are not specified for the out parameters since these - /// methods do a simple conversion operation from the packet string. Please - /// consult the appropriate sensor user manual for details about - /// the units returned by the sensor. - /// - /// \{ - - /// \brief Parses a VNYPR asynchronous packet. - /// - /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. - void parseVNYPR(vn::math::vec3f *yawPitchRoll); - - /// \brief Parses a VNQTN asynchronous packet. - /// - /// \param[out] quaternion The quaternion values in the packet. - void parseVNQTN(vn::math::vec4f *quaternion); - - #ifdef INTERNAL - - /// \brief Parses a VNQTM asynchronous packet. - /// - /// \param[out] quaternion The quaternion values in the packet. - /// \param[out] magnetic The magnetic values in the packet. - void parseVNQTM(math::vec4f *quaternion, math::vec3f *magnetic); - - /// \brief Parses a VNQTA asynchronous packet. - /// - /// \param[out] quaternion The quaternion values in the packet. - /// \param[out] acceleration The acceleration values in the packet. - void parseVNQTA(math::vec4f *quaternion, math::vec3f *acceleration); - - /// \brief Parses a VNQTR asynchronous packet. - /// - /// \param[out] quaternion The quaternion values in the packet. - /// \param[out] angularRate The angular rate values in the packet. - void parseVNQTR(math::vec4f *quaternion, math::vec3f *angularRate); - - /// \brief Parses a VNQMA asynchronous packet. - /// - /// \param[out] quaternion The quaternion values in the packet. - /// \param[out] magnetic The magnetic values in the packet. - /// \param[out] acceleration The acceleration values in the packet. - void parseVNQMA(math::vec4f *quaternion, math::vec3f *magnetic, math::vec3f *acceleration); - - /// \brief Parses a VNQAR asynchronous packet. - /// - /// \param[out] quaternion The quaternion values in the packet. - /// \param[out] acceleration The acceleration values in the packet. - /// \param[out] angularRate The angular rate values in the packet. - void parseVNQAR(math::vec4f *quaternion, math::vec3f *acceleration, math::vec3f *angularRate); - - #endif - - /// \brief Parses a VNQMR asynchronous packet. - /// - /// \param[out] quaternion The quaternion values in the packet. - /// \param[out] magnetic The magnetic values in the packet. - /// \param[out] acceleration The acceleration values in the packet. - /// \param[out] angularRate The angular rate values in the packet. - void parseVNQMR(vn::math::vec4f *quaternion, vn::math::vec3f *magnetic, vn::math::vec3f *acceleration, vn::math::vec3f *angularRate); - - #ifdef INTERNAL - - /// \brief Parses a VNDCM asynchronous packet. - /// - /// \param[out] dcm The directional cosine matrix values in the packet. - void parseVNDCM(math::mat3f *dcm); - - #endif - - /// \brief Parses a VNMAG asynchronous packet. - /// - /// \param[out] magnetic The magnetic values in the packet. - void parseVNMAG(vn::math::vec3f *magnetic); - - /// \brief Parses a VNACC asynchronous packet. - /// - /// \param[out] acceleration The acceleration values in the packet. - void parseVNACC(vn::math::vec3f *acceleration); - - /// \brief Parses a VNGYR asynchronous packet. - /// - /// \param[out] angularRate The angular rate values in the packet. - void parseVNGYR(vn::math::vec3f *angularRate); - - /// \brief Parses a VNMAR asynchronous packet. - /// - /// \param[out] magnetic The magnetic values in the packet. - /// \param[out] acceleration The acceleration values in the packet. - /// \param[out] angularRate The angular rate values in the packet. - void parseVNMAR(vn::math::vec3f *magnetic, vn::math::vec3f *acceleration, vn::math::vec3f *angularRate); - - /// \brief Parses a VNYMR asynchronous packet. - /// - /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. - /// \param[out] magnetic The magnetic values in the packet. - /// \param[out] acceleration The acceleration values in the packet. - /// \param[out] angularRate The angular rate values in the packet. - void parseVNYMR(vn::math::vec3f *yawPitchRoll, vn::math::vec3f *magnetic, vn::math::vec3f *acceleration, vn::math::vec3f *angularRate); - - #ifdef INTERNAL - - /// \brief Parses a VNYCM asynchronous packet. - /// - /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. - /// \param[out] magnetic The magnetic values in the packet. - /// \param[out] acceleration The acceleration values in the packet. - /// \param[out] angularRate The angular rate values in the packet. - /// \param[out] temperature The temperature value in the packet. - void parseVNYCM(math::vec3f *yawPitchRoll, math::vec3f *magnetic, math::vec3f *acceleration, math::vec3f *angularRate, float *temperature); - - #endif - - /// \brief Parses a VNYBA asynchronous packet. - /// - /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. - /// \param[out] accelerationBody The acceleration body values in the packet. - /// \param[out] angularRate The angular rate values in the packet. - void parseVNYBA(vn::math::vec3f *yawPitchRoll, vn::math::vec3f *accelerationBody, vn::math::vec3f *angularRate); - - /// \brief Parses a VNYIA asynchronous packet. - /// - /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. - /// \param[out] accelerationInertial The acceleration inertial values in the packet. - /// \param[out] angularRate The angular rate values in the packet. - void parseVNYIA(vn::math::vec3f *yawPitchRoll, vn::math::vec3f *accelerationInertial, vn::math::vec3f *angularRate); - - #ifdef INTERNAL - - /// \brief Parses a VNICM asynchronous packet. - /// - /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. - /// \param[out] magnetic The magnetic values in the packet. - /// \param[out] accelerationInertial The acceleration inertial values in the packet. - /// \param[out] angularRate The angular rate values in the packet. - void parseVNICM(math::vec3f *yawPitchRoll, math::vec3f *magnetic, math::vec3f *accelerationInertial, math::vec3f *angularRate); - - #endif - - /// \brief Parses a VNIMU asynchronous packet. - /// - /// \param[out] magneticUncompensated The uncompensated magnetic values in the packet. - /// \param[out] accelerationUncompensated The uncompensated acceleration values in the packet. - /// \param[out] angularRateUncompensated The uncompensated angular rate values in the packet. - /// \param[out] temperature The temperature value in the packet. - /// \param[out] pressure The pressure value in the packet. - void parseVNIMU(vn::math::vec3f *magneticUncompensated, vn::math::vec3f *accelerationUncompensated, vn::math::vec3f *angularRateUncompensated, float *temperature, float *pressure); - - /// \brief Parses a VNGPS asynchronous packet. - /// - /// \param[out] time The time value in the packet. - /// \param[out] week The week value in the packet. - /// \param[out] gpsFix The GPS fix value in the packet. - /// \param[out] numSats The NumSats value in the packet. - /// \param[out] lla The latitude, longitude and altitude values in the packet. - /// \param[out] nedVel The NED velocity values in the packet. - /// \param[out] nedAcc The NED position accuracy values in the packet. - /// \param[out] speedAcc The SpeedAcc value in the packet. - /// \param[out] timeAcc The TimeAcc value in the packet. - void parseVNGPS(double *time, uint16_t *week, uint8_t *gpsFix, uint8_t *numSats, vn::math::vec3d *lla, vn::math::vec3f *nedVel, vn::math::vec3f *nedAcc, float *speedAcc, float *timeAcc); - - /// \brief Parses a VNINS asynchronous packet. - /// - /// \param[out] time The time value in the packet. - /// \param[out] week The week value in the packet. - /// \param[out] status The status value in the packet. - /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. - /// \param[out] lla The latitude, longitude, altitude values in the packet. - /// \param[out] nedVel The NED velocity values in the packet. - /// \param[out] attUncertainty The attitude uncertainty value in the packet. - /// \param[out] posUncertainty The position uncertainty value in the packet. - /// \param[out] velUncertainty The velocity uncertainty value in the packet. - void parseVNINS(double *time, uint16_t *week, uint16_t *status, vn::math::vec3f *yawPitchRoll, vn::math::vec3d *lla, vn::math::vec3f *nedVel, float *attUncertainty, float *posUncertainty, float *velUncertainty); - - /// \brief Parses a VNINE asynchronous packet. - /// - /// \param[out] time The time value in the packet. - /// \param[out] week The week value in the packet. - /// \param[out] status The status value in the packet. - /// \param[out] ypr The yaw, pitch, roll values in the packet. - /// \param[out] position The ECEF position values in the packet. - /// \param[out] velocity The ECEF velocity values in the packet. - /// \param[out] attUncertainty The attitude uncertainty value in the packet. - /// \param[out] posUncertainty The position uncertainty value in the packet. - /// \param[out] velUncertainty The velocity uncertainty value in the packet. - void parseVNINE(double *time, uint16_t *week, uint16_t *status, vn::math::vec3f *ypr, vn::math::vec3d *position, vn::math::vec3f *velocity, float *attUncertainty, float *posUncertainty, float *velUncertainty); - - /// \brief Parses a VNISL asynchronous packet. - /// - /// \param[out] ypr The yaw, pitch, roll values in the packet. - /// \param[out] lla The latitude, longitude, altitude values in the packet. - /// \param[out] velocity The velocity values in the packet. - /// \param[out] acceleration The acceleration values in the packet. - /// \param[out] angularRate The angular rate values in the packet. - void parseVNISL(vn::math::vec3f* ypr, vn::math::vec3d* lla, vn::math::vec3f* velocity, vn::math::vec3f* acceleration, vn::math::vec3f* angularRate); - - /// \brief Parses a VNISE asynchronous packet. - /// - /// \param[out] ypr The yaw, pitch, roll values in the packet. - /// \param[out] position The ECEF position values in the packet. - /// \param[out] velocity The ECEF velocity values in the packet. - /// \param[out] acceleration The acceleration values in the packet. - /// \param[out] angularRate The angular rate values in the packet. - void parseVNISE(vn::math::vec3f* ypr, vn::math::vec3d* position, vn::math::vec3f* velocity, vn::math::vec3f* acceleration, vn::math::vec3f* angularRate); - - #ifdef INTERNAL - - /// \brief Parses a VNRAW asynchronous packet. - /// - /// \param[out] magneticVoltage The magnetic voltage values in the packet. - /// \param[out] accelerationVoltage The acceleration voltage values in the packet. - /// \param[out] angularRateVoltage The angular rate voltage values in the packet. - /// \param[out] temperatureVoltage The temperature voltage value in the packet. - void parseVNRAW(math::vec3f *magneticVoltage, math::vec3f *accelerationVoltage, math::vec3f *angularRateVoltage, float *temperatureVoltage); - - /// \brief Parses a VNCMV asynchronous packet. - /// - /// \param[out] magneticUncompensated The uncompensated magnetic values in the packet. - /// \param[out] accelerationUncompensated The uncompensated acceleration values in the packet. - /// \param[out] angularRateUncompensated The uncompensated angular rate values in the packet. - /// \param[out] temperature The temperature value in the packet. - void parseVNCMV(math::vec3f *magneticUncompensated, math::vec3f *accelerationUncompensated, math::vec3f *angularRateUncompensated, float *temperature); - - /// \brief Parses a VNSTV asynchronous packet. - /// - /// \param[out] quaternion The quaternion values in the packet. - /// \param[out] angularRateBias The angular rate bias values in the packet. - void parseVNSTV(math::vec4f *quaternion, math::vec3f *angularRateBias); - - /// \brief Parses a VNCOV asynchronous packet. - /// - /// \param[out] attitudeVariance The attitude variance values in the packet. - /// \param[out] angularRateBiasVariance The angular rate bias variance values in the packet. - void parseVNCOV(math::vec3f *attitudeVariance, math::vec3f *angularRateBiasVariance); - - #endif - - /// \brief Parses a VNGPE asynchronous packet. - /// - /// \param[out] tow The tow value in the packet. - /// \param[out] week The week value in the packet. - /// \param[out] gpsFix The GPS fix value in the packet. - /// \param[out] numSats The numSats value in the packet. - /// \param[out] position The ECEF position values in the packet. - /// \param[out] velocity The ECEF velocity values in the packet. - /// \param[out] posAcc The PosAcc values in the packet. - /// \param[out] speedAcc The SpeedAcc value in the packet. - /// \param[out] timeAcc The TimeAcc value in the packet. - void parseVNGPE(double *tow, uint16_t *week, uint8_t *gpsFix, uint8_t *numSats, vn::math::vec3d *position, vn::math::vec3f *velocity, vn::math::vec3f *posAcc, float *speedAcc, float *timeAcc); - - /// \brief Parses a VNDTV asynchronous packet. - /// - /// \param[out] deltaTime The DeltaTime value in the packet. - /// \param[out] deltaTheta The DeltaTheta values in the packet. - /// \param[out] deltaVelocity The DeltaVelocity values in the packet. - void parseVNDTV(float *deltaTime, vn::math::vec3f *deltaTheta, vn::math::vec3f *deltaVelocity); - - /// \} - - /// \defgroup uartAsciiResponseParsers UART ASCII Response Packet Parsers - /// \brief This group of methods allow parsing of ASCII response data - /// packets from VectorNav's sensors. - /// - /// The units are not specified for the out parameters since these - /// methods do a simple conversion operation from the packet string. Please - /// consult the appropriate user manual for your sensor for details about - /// the units returned by the sensor. - /// - /// \{ - - /// \brief Parses a response from reading any of the Binary Output registers. - /// - /// \param[out] asyncMode The register's AsyncMode field. - /// \param[out] rateDivisor The register's RateDivisor field. - /// \param[out] outputGroup The register's OutputGroup field. - /// \param[out] commonField The set fields of Output Group 1 (Common) if present. - /// \param[out] timeField The set fields of Output Group 2 (Time) if present. - /// \param[out] imuField The set fields of Output Group 3 (IMU) if present. - /// \param[out] gpsField The set fields of Output Group 4 (GPS) if present. - /// \param[out] attitudeField The set fields of Output Group 5 (Attitude) if present. + static size_t genWriteBinaryOutput3( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint16_t asyncMode, + uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, uint16_t imuField, + uint16_t gpsField, uint16_t attitudeField, uint16_t insField, uint16_t gps2Field); + + /// \brief Generates a command to write sensor settings to non-volatile memory. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genWriteSettings(ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to tare the sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genTare(ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to alert the sensor of a known magnetic disturbance. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] indicates if a known magnetic disturbance is present or not. + /// \return The total number bytes in the generated command. + static size_t genKnownMagneticDisturbance( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, + bool isMagneticDisturbancePresent); + + /// \brief Generates a command to alert the sensor of a known acceleration disturbance. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] indicates if a known acceleration disturbance is present or not. + /// \return The total number bytes in the generated command. + static size_t genKnownAccelerationDisturbance( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, + bool isAccelerationDisturbancePresent); + + /// \brief Generates a command to set the gyro bias. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genSetGyroBias(ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to retore factory settings. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genRestoreFactorySettings( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to reset the sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReset(ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to put the sensor in firmware update mode. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genFirmwareUpdate( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the Serial Baud Rate register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] port The port to read from. + /// \return The total number bytes in the generated command. + static size_t genReadSerialBaudRate( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t port); + + /// \brief Generates a command to write to the Serial Baud Rate register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] baudrate The register's Baud Rate field. + /// \param[in] port The port to write to. + /// \return The total number bytes in the generated command. + static size_t genWriteSerialBaudRate( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint32_t baudrate, + uint8_t port); + + /// \brief Generates a command to read the Async Data Output Type register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] port The port to read from. + /// \return The total number bytes in the generated command. + static size_t genReadAsyncDataOutputType( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t port); + + /// \brief Generates a command to write to the Async Data Output Type register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] ador The register's ADOR field. + /// \param[in] port The port to write to. + /// \return The total number bytes in the generated command. + static size_t genWriteAsyncDataOutputType( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint32_t ador, uint8_t port); + + /// \brief Generates a command to read the Async Data Output Frequency register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] port The port to read from. + /// \return The total number bytes in the generated command. + static size_t genReadAsyncDataOutputFrequency( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t port); + + /// \brief Generates a command to write to the Async Data Output Frequency register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] adof The register's ADOF field. + /// \param[in] port The port to write to. + /// \return The total number bytes in the generated command. + static size_t genWriteAsyncDataOutputFrequency( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint32_t adof, uint8_t port); + + /// \brief Generates a command to write to a firmware update record on a VectorNav sensor to the bootloader. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] record The record to write to the bootloader for a specific processor on the sensor. + /// \return The total number bytes in the generated command. + static size_t genWriteFirmwareUpdateRecord( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, std::string record); + + /// \brief Generates a command to read the User Tag register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadUserTag(ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the User Tag register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] tag The register's Tag field. + /// \return The total number bytes in the generated command. + static size_t genWriteUserTag( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, std::string tag); + + /// \brief Generates a command to read the Model Number register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadModelNumber( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the Hardware Revision register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadHardwareRevision( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the Serial Number register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadSerialNumber( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the Firmware Version register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadFirmwareVersion( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the Serial Baud Rate register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadSerialBaudRate( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Serial Baud Rate register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] baudrate The register's Baud Rate field. + /// \return The total number bytes in the generated command. + static size_t genWriteSerialBaudRate( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint32_t baudrate); + + /// \brief Generates a command to read the Async Data Output Type register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadAsyncDataOutputType( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Async Data Output Type register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] ador The register's ADOR field. + /// \return The total number bytes in the generated command. + static size_t genWriteAsyncDataOutputType( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint32_t ador); + + /// \brief Generates a command to read the Async Data Output Frequency register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadAsyncDataOutputFrequency( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Async Data Output Frequency register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] adof The register's ADOF field. + /// \return The total number bytes in the generated command. + static size_t genWriteAsyncDataOutputFrequency( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint32_t adof); + + /// \brief Generates a command to read the Yaw Pitch Roll register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadYawPitchRoll( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the Attitude Quaternion register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadAttitudeQuaternion( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the Quaternion, Magnetic, Acceleration and Angular Rates register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadQuaternionMagneticAccelerationAndAngularRates( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the Magnetic Measurements register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadMagneticMeasurements( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the Acceleration Measurements register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadAccelerationMeasurements( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the Angular Rate Measurements register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadAngularRateMeasurements( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the Magnetic, Acceleration and Angular Rates register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadMagneticAccelerationAndAngularRates( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the Magnetic and Gravity Reference Vectors register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadMagneticAndGravityReferenceVectors( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Magnetic and Gravity Reference Vectors register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] magRef The register's MagRef field. + /// \param[in] accRef The register's AccRef field. + /// \return The total number bytes in the generated command. + static size_t genWriteMagneticAndGravityReferenceVectors( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vn::math::vec3f magRef, + vn::math::vec3f accRef); + + /// \brief Generates a command to read the Filter Measurements Variance Parameters register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadFilterMeasurementsVarianceParameters( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Filter Measurements Variance Parameters register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] angularWalkVariance The register's Angular Walk Variance field. + /// \param[in] angularRateVariance The register's Angular Rate Variance field. + /// \param[in] magneticVariance The register's Magnetic Variance field. + /// \param[in] accelerationVariance The register's Acceleration Variance field. + /// \return The total number bytes in the generated command. + static size_t genWriteFilterMeasurementsVarianceParameters( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, float angularWalkVariance, + vn::math::vec3f angularRateVariance, vn::math::vec3f magneticVariance, + vn::math::vec3f accelerationVariance); + + /// \brief Generates a command to read the Magnetometer Compensation register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadMagnetometerCompensation( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Magnetometer Compensation register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] c The register's C field. + /// \param[in] b The register's B field. + /// \return The total number bytes in the generated command. + static size_t genWriteMagnetometerCompensation( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vn::math::mat3f c, + vn::math::vec3f b); + + /// \brief Generates a command to read the Filter Active Tuning Parameters register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadFilterActiveTuningParameters( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Filter Active Tuning Parameters register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] magneticDisturbanceGain The register's Magnetic Disturbance Gain field. + /// \param[in] accelerationDisturbanceGain The register's Acceleration Disturbance Gain field. + /// \param[in] magneticDisturbanceMemory The register's Magnetic Disturbance Memory field. + /// \param[in] accelerationDisturbanceMemory The register's Acceleration Disturbance Memory field. + /// \return The total number bytes in the generated command. + static size_t genWriteFilterActiveTuningParameters( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, + float magneticDisturbanceGain, float accelerationDisturbanceGain, + float magneticDisturbanceMemory, float accelerationDisturbanceMemory); + + /// \brief Generates a command to read the Acceleration Compensation register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadAccelerationCompensation( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Acceleration Compensation register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] c The register's C field. + /// \param[in] b The register's B field. + /// \return The total number bytes in the generated command. + static size_t genWriteAccelerationCompensation( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vn::math::mat3f c, + vn::math::vec3f b); + + /// \brief Generates a command to read the Reference Frame Rotation register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadReferenceFrameRotation( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Reference Frame Rotation register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] c The register's C field. + /// \return The total number bytes in the generated command. + static size_t genWriteReferenceFrameRotation( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vn::math::mat3f c); + + /// \brief Generates a command to read the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadYawPitchRollMagneticAccelerationAndAngularRates( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the Communication Protocol Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadCommunicationProtocolControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Communication Protocol Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] serialCount The register's SerialCount field. + /// \param[in] serialStatus The register's SerialStatus field. + /// \param[in] spiCount The register's SPICount field. + /// \param[in] spiStatus The register's SPIStatus field. + /// \param[in] serialChecksum The register's SerialChecksum field. + /// \param[in] spiChecksum The register's SPIChecksum field. + /// \param[in] errorMode The register's ErrorMode field. + /// \return The total number bytes in the generated command. + static size_t genWriteCommunicationProtocolControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t serialCount, + uint8_t serialStatus, uint8_t spiCount, uint8_t spiStatus, uint8_t serialChecksum, + uint8_t spiChecksum, uint8_t errorMode); + + /// \brief Generates a command to read the Synchronization Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadSynchronizationControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Synchronization Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] syncInMode The register's SyncInMode field. + /// \param[in] syncInEdge The register's SyncInEdge field. + /// \param[in] syncInSkipFactor The register's SyncInSkipFactor field. + /// \param[in] syncOutMode The register's SyncOutMode field. + /// \param[in] syncOutPolarity The register's SyncOutPolarity field. + /// \param[in] syncOutSkipFactor The register's SyncOutSkipFactor field. + /// \param[in] syncOutPulseWidth The register's SyncOutPulseWidth field. + /// \return The total number bytes in the generated command. + static size_t genWriteSynchronizationControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t syncInMode, + uint8_t syncInEdge, uint16_t syncInSkipFactor, uint8_t syncOutMode, uint8_t syncOutPolarity, + uint16_t syncOutSkipFactor, uint32_t syncOutPulseWidth); + + /// \brief Generates a command to read the Synchronization Status register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadSynchronizationStatus( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Synchronization Status register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] syncInCount The register's SyncInCount field. + /// \param[in] syncInTime The register's SyncInTime field. + /// \param[in] syncOutCount The register's SyncOutCount field. + /// \return The total number bytes in the generated command. + static size_t genWriteSynchronizationStatus( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint32_t syncInCount, + uint32_t syncInTime, uint32_t syncOutCount); + + /// \brief Generates a command to read the Filter Basic Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadFilterBasicControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Filter Basic Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] magMode The register's MagMode field. + /// \param[in] extMagMode The register's ExtMagMode field. + /// \param[in] extAccMode The register's ExtAccMode field. + /// \param[in] extGyroMode The register's ExtGyroMode field. + /// \param[in] gyroLimit The register's GyroLimit field. + /// \return The total number bytes in the generated command. + static size_t genWriteFilterBasicControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t magMode, + uint8_t extMagMode, uint8_t extAccMode, uint8_t extGyroMode, vn::math::vec3f gyroLimit); + + /// \brief Generates a command to read the Heave Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadHeaveConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the VPE Basic Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] initialWavePeriod Value for the initialWavePeriod field. + /// \param[in] initialWaveAmplitude Value for the initialWaveAmplitude field. + /// \param[in] maxWavePeriod Value for the maxWavePeriod field. + /// \param[in] minWaveAmplitude Value for the minWaveAmplitude field. + /// \param[in] delayedHeaveCutoffFreq Value for the delayedHeaveCutoffFreq field. + /// \param[in] heaveCutoffFreq Value for the heaveCutoffFreq field. + /// \param[in] heaveRateCutoffFreq Value for the heaveRateCutoffFreq field. + /// \return The total number bytes in the generated command. + static size_t genWriteHeaveConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, float initialWavePeriod, + float initialWaveAmplitude, float maxWavePeriod, float minWaveAmplitude, + float delayedHeaveCutoffFreq, float heaveCutoffFreq, float heaveRateCutoffFreq); + + /// \brief Generates a command to read the VPE Basic Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadVpeBasicControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the VPE Basic Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] enable The register's Enable field. + /// \param[in] headingMode The register's HeadingMode field. + /// \param[in] filteringMode The register's FilteringMode field. + /// \param[in] tuningMode The register's TuningMode field. + /// \return The total number bytes in the generated command. + static size_t genWriteVpeBasicControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t enable, + uint8_t headingMode, uint8_t filteringMode, uint8_t tuningMode); + + /// \brief Generates a command to read the VPE Magnetometer Basic Tuning register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadVpeMagnetometerBasicTuning( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the VPE Magnetometer Basic Tuning register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] baseTuning The register's BaseTuning field. + /// \param[in] adaptiveTuning The register's AdaptiveTuning field. + /// \param[in] adaptiveFiltering The register's AdaptiveFiltering field. + /// \return The total number bytes in the generated command. + static size_t genWriteVpeMagnetometerBasicTuning( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vn::math::vec3f baseTuning, + vn::math::vec3f adaptiveTuning, vn::math::vec3f adaptiveFiltering); + + /// \brief Generates a command to read the VPE Magnetometer Advanced Tuning register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadVpeMagnetometerAdvancedTuning( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the VPE Magnetometer Advanced Tuning register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] minFiltering The register's MinFiltering field. + /// \param[in] maxFiltering The register's MaxFiltering field. + /// \param[in] maxAdaptRate The register's MaxAdaptRate field. + /// \param[in] disturbanceWindow The register's DisturbanceWindow field. + /// \param[in] maxTuning The register's MaxTuning field. + /// \return The total number bytes in the generated command. + static size_t genWriteVpeMagnetometerAdvancedTuning( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vn::math::vec3f minFiltering, + vn::math::vec3f maxFiltering, float maxAdaptRate, float disturbanceWindow, float maxTuning); + + /// \brief Generates a command to read the VPE Accelerometer Basic Tuning register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadVpeAccelerometerBasicTuning( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the VPE Accelerometer Basic Tuning register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] baseTuning The register's BaseTuning field. + /// \param[in] adaptiveTuning The register's AdaptiveTuning field. + /// \param[in] adaptiveFiltering The register's AdaptiveFiltering field. + /// \return The total number bytes in the generated command. + static size_t genWriteVpeAccelerometerBasicTuning( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vn::math::vec3f baseTuning, + vn::math::vec3f adaptiveTuning, vn::math::vec3f adaptiveFiltering); + + /// \brief Generates a command to read the VPE Accelerometer Advanced Tuning register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadVpeAccelerometerAdvancedTuning( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the VPE Accelerometer Advanced Tuning register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] minFiltering The register's MinFiltering field. + /// \param[in] maxFiltering The register's MaxFiltering field. + /// \param[in] maxAdaptRate The register's MaxAdaptRate field. + /// \param[in] disturbanceWindow The register's DisturbanceWindow field. + /// \param[in] maxTuning The register's MaxTuning field. + /// \return The total number bytes in the generated command. + static size_t genWriteVpeAccelerometerAdvancedTuning( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vn::math::vec3f minFiltering, + vn::math::vec3f maxFiltering, float maxAdaptRate, float disturbanceWindow, float maxTuning); + + /// \brief Generates a command to read the VPE Gyro Basic Tuning register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadVpeGyroBasicTuning( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the VPE Gyro Basic Tuning register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] angularWalkVariance The register's AngularWalkVariance field. + /// \param[in] baseTuning The register's BaseTuning field. + /// \param[in] adaptiveTuning The register's AdaptiveTuning field. + /// \return The total number bytes in the generated command. + static size_t genWriteVpeGyroBasicTuning( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, + vn::math::vec3f angularWalkVariance, vn::math::vec3f baseTuning, + vn::math::vec3f adaptiveTuning); + + /// \brief Generates a command to read the Filter Startup Gyro Bias register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadFilterStartupGyroBias( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Filter Startup Gyro Bias register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] bias The register's Bias field. + /// \return The total number bytes in the generated command. + static size_t genWriteFilterStartupGyroBias( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vn::math::vec3f bias); + + /// \brief Generates a command to read the Magnetometer Calibration Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadMagnetometerCalibrationControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Magnetometer Calibration Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] hsiMode The register's HSIMode field. + /// \param[in] hsiOutput The register's HSIOutput field. + /// \param[in] convergeRate The register's ConvergeRate field. + /// \return The total number bytes in the generated command. + static size_t genWriteMagnetometerCalibrationControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t hsiMode, + uint8_t hsiOutput, uint8_t convergeRate); + + /// \brief Generates a command to read the Calculated Magnetometer Calibration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadCalculatedMagnetometerCalibration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the Indoor Heading Mode Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadIndoorHeadingModeControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Indoor Heading Mode Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] maxRateError The register's Max Rate Error field. + /// \return The total number bytes in the generated command. + static size_t genWriteIndoorHeadingModeControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, float maxRateError); + + /// \brief Generates a command to read the Velocity Compensation Measurement register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadVelocityCompensationMeasurement( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Velocity Compensation Measurement register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] velocity The register's Velocity field. + /// \return The total number bytes in the generated command. + static size_t genWriteVelocityCompensationMeasurement( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vn::math::vec3f velocity); + + /// \brief Generates a command to read the Velocity Compensation Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadVelocityCompensationControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Velocity Compensation Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] mode The register's Mode field. + /// \param[in] velocityTuning The register's VelocityTuning field. + /// \param[in] rateTuning The register's RateTuning field. + /// \return The total number bytes in the generated command. + static size_t genWriteVelocityCompensationControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t mode, + float velocityTuning, float rateTuning); + + /// \brief Generates a command to read the Velocity Compensation Status register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadVelocityCompensationStatus( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the IMU Measurements register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadImuMeasurements( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the GPS Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadGpsConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the GPS Configuration register on a VectorNav sensor (deprecated). + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] mode The register's Mode field. + /// \param[in] ppsSource The register's PpsSource field. + /// \return The total number bytes in the generated command. + static size_t genWriteGpsConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t mode, + uint8_t ppsSource); + + /// \brief Generates a command to write to the GPS Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] mode The register's Mode field. + /// \param[in] ppsSource The register's PpsSource field. + /// \param[in] rate The register's Rate field. + /// \param[in] antPow The register's AntPower field. + /// \return The total number bytes in the generated command. + static size_t genWriteGpsConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t mode, + uint8_t ppsSource, uint8_t rate, uint8_t antPow); + + /// \brief Generates a command to read the GPS Antenna Offset register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadGpsAntennaOffset( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the GPS Antenna Offset register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] position The register's Position field. + /// \return The total number bytes in the generated command. + static size_t genWriteGpsAntennaOffset( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vn::math::vec3f position); + + /// \brief Generates a command to read the GPS Solution - LLA register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadGpsSolutionLla( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the GPS Solution - ECEF register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadGpsSolutionEcef( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the INS Solution - LLA register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadInsSolutionLla( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the INS Solution - ECEF register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadInsSolutionEcef( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the INS Basic Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadInsBasicConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the INS Basic Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] scenario The register's Scenario field. + /// \param[in] ahrsAiding The register's AhrsAiding field. + /// \param[in] estBaseline The register's EstBaseline field. + /// \return The total number bytes in the generated command. + static size_t genWriteInsBasicConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t scenario, + uint8_t ahrsAiding, uint8_t estBaseline); + + /// \brief Generates a command to read the INS Advanced Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadInsAdvancedConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the INS Advanced Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] useMag The register's UseMag field. + /// \param[in] usePres The register's UsePres field. + /// \param[in] posAtt The register's PosAtt field. + /// \param[in] velAtt The register's VelAtt field. + /// \param[in] velBias The register's VelBias field. + /// \param[in] useFoam The register's UseFoam field. + /// \param[in] gpsCovType The register's GPSCovType field. + /// \param[in] velCount The register's VelCount field. + /// \param[in] velInit The register's VelInit field. + /// \param[in] moveOrigin The register's MoveOrigin field. + /// \param[in] gpsTimeout The register's GPSTimeout field. + /// \param[in] deltaLimitPos The register's DeltaLimitPos field. + /// \param[in] deltaLimitVel The register's DeltaLimitVel field. + /// \param[in] minPosUncertainty The register's MinPosUncertainty field. + /// \param[in] minVelUncertainty The register's MinVelUncertainty field. + /// \return The total number bytes in the generated command. + static size_t genWriteInsAdvancedConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t useMag, + uint8_t usePres, uint8_t posAtt, uint8_t velAtt, uint8_t velBias, uint8_t useFoam, + uint8_t gpsCovType, uint8_t velCount, float velInit, float moveOrigin, float gpsTimeout, + float deltaLimitPos, float deltaLimitVel, float minPosUncertainty, float minVelUncertainty); + + /// \brief Generates a command to read the INS State - LLA register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadInsStateLla( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the INS State - ECEF register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadInsStateEcef( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the Startup Filter Bias Estimate register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadStartupFilterBiasEstimate( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Startup Filter Bias Estimate register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] gyroBias The register's GyroBias field. + /// \param[in] accelBias The register's AccelBias field. + /// \param[in] pressureBias The register's PressureBias field. + /// \return The total number bytes in the generated command. + static size_t genWriteStartupFilterBiasEstimate( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vn::math::vec3f gyroBias, + vn::math::vec3f accelBias, float pressureBias); + + /// \brief Generates a command to read the Delta Theta and Delta Velocity register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadDeltaThetaAndDeltaVelocity( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the Delta Theta and Delta Velocity Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadDeltaThetaAndDeltaVelocityConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Delta Theta and Delta Velocity Configuration register on a VectorNav sensor (deprecated). + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] integrationFrame The register's IntegrationFrame field. + /// \param[in] gyroCompensation The register's GyroCompensation field. + /// \param[in] accelCompensation The register's AccelCompensation field. + /// \return The total number bytes in the generated command. + static size_t genWriteDeltaThetaAndDeltaVelocityConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t integrationFrame, + uint8_t gyroCompensation, uint8_t accelCompensation); + + /// \brief Generates a command to write to the Delta Theta and Delta Velocity Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] integrationFrame The register's IntegrationFrame field. + /// \param[in] gyroCompensation The register's GyroCompensation field. + /// \param[in] accelCompensation The register's AccelCompensation field. + /// \param[in] earthRateCorrection The register's EarthRateCorrection field. + /// \return The total number bytes in the generated command. + static size_t genWriteDeltaThetaAndDeltaVelocityConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t integrationFrame, + uint8_t gyroCompensation, uint8_t accelCompensation, uint8_t earthRateCorrection); + + /// \brief Generates a command to read the Reference Vector Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadReferenceVectorConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Reference Vector Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] useMagModel The register's UseMagModel field. + /// \param[in] useGravityModel The register's UseGravityModel field. + /// \param[in] recalcThreshold The register's RecalcThreshold field. + /// \param[in] year The register's Year field. + /// \param[in] position The register's Position field. + /// \return The total number bytes in the generated command. + static size_t genWriteReferenceVectorConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t useMagModel, + uint8_t useGravityModel, uint32_t recalcThreshold, float year, vn::math::vec3d position); + + /// \brief Generates a command to read the Gyro Compensation register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadGyroCompensation( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the Gyro Compensation register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] c The register's C field. + /// \param[in] b The register's B field. + /// \return The total number bytes in the generated command. + static size_t genWriteGyroCompensation( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vn::math::mat3f c, + vn::math::vec3f b); + + /// \brief Generates a command to read the IMU Filtering Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadImuFilteringConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the IMU Filtering Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] magWindowSize The register's MagWindowSize field. + /// \param[in] accelWindowSize The register's AccelWindowSize field. + /// \param[in] gyroWindowSize The register's GyroWindowSize field. + /// \param[in] tempWindowSize The register's TempWindowSize field. + /// \param[in] presWindowSize The register's PresWindowSize field. + /// \param[in] magFilterMode The register's MagFilterMode field. + /// \param[in] accelFilterMode The register's AccelFilterMode field. + /// \param[in] gyroFilterMode The register's GyroFilterMode field. + /// \param[in] tempFilterMode The register's TempFilterMode field. + /// \param[in] presFilterMode The register's PresFilterMode field. + /// \return The total number bytes in the generated command. + static size_t genWriteImuFilteringConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, 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 Generates a command to read the GPS Compass Baseline register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadGpsCompassBaseline( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the GPS Compass Baseline register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] position The register's Position field. + /// \param[in] uncertainty The register's Uncertainty field. + /// \return The total number bytes in the generated command. + static size_t genWriteGpsCompassBaseline( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vn::math::vec3f position, + vn::math::vec3f uncertainty); + + /// \brief Generates a command to read the GPS Compass Estimated Baseline register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadGpsCompassEstimatedBaseline( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the IMU Rate Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadImuRateConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to write to the IMU Rate Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] imuRate The register's imuRate field. + /// \param[in] navDivisor The register's NavDivisor field. + /// \param[in] filterTargetRate The register's filterTargetRate field. + /// \param[in] filterMinRate The register's filterMinRate field. + /// \return The total number bytes in the generated command. + static size_t genWriteImuRateConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint16_t imuRate, + uint16_t navDivisor, float filterTargetRate, float filterMinRate); + + /// \brief Generates a command to read the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadYawPitchRollTrueBodyAccelerationAndAngularRates( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \brief Generates a command to read the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadYawPitchRollTrueInertialAccelerationAndAngularRates( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size); + + /// \} + + /// \defgroup uartPacketAsciiAsyncParsers UART ASCII Asynchronous Packet Parsers + /// \brief This group of methods allow parsing of ASCII asynchronous data + /// packets from VectorNav sensors. + /// + /// The units are not specified for the out parameters since these + /// methods do a simple conversion operation from the packet string. Please + /// consult the appropriate sensor user manual for details about + /// the units returned by the sensor. + /// + /// \{ + + /// \brief Parses a VNYPR asynchronous packet. + /// + /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + void parseVNYPR(vn::math::vec3f * yawPitchRoll); + + /// \brief Parses a VNQTN asynchronous packet. + /// + /// \param[out] quaternion The quaternion values in the packet. + void parseVNQTN(vn::math::vec4f * quaternion); + +#ifdef INTERNAL + + /// \brief Parses a VNQTM asynchronous packet. + /// + /// \param[out] quaternion The quaternion values in the packet. + /// \param[out] magnetic The magnetic values in the packet. + void parseVNQTM(math::vec4f * quaternion, math::vec3f * magnetic); + + /// \brief Parses a VNQTA asynchronous packet. + /// + /// \param[out] quaternion The quaternion values in the packet. + /// \param[out] acceleration The acceleration values in the packet. + void parseVNQTA(math::vec4f * quaternion, math::vec3f * acceleration); + + /// \brief Parses a VNQTR asynchronous packet. + /// + /// \param[out] quaternion The quaternion values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + void parseVNQTR(math::vec4f * quaternion, math::vec3f * angularRate); + + /// \brief Parses a VNQMA asynchronous packet. + /// + /// \param[out] quaternion The quaternion values in the packet. + /// \param[out] magnetic The magnetic values in the packet. + /// \param[out] acceleration The acceleration values in the packet. + void parseVNQMA(math::vec4f * quaternion, math::vec3f * magnetic, math::vec3f * acceleration); + + /// \brief Parses a VNQAR asynchronous packet. + /// + /// \param[out] quaternion The quaternion values in the packet. + /// \param[out] acceleration The acceleration values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + void parseVNQAR(math::vec4f * quaternion, math::vec3f * acceleration, math::vec3f * angularRate); + +#endif + + /// \brief Parses a VNQMR asynchronous packet. + /// + /// \param[out] quaternion The quaternion values in the packet. + /// \param[out] magnetic The magnetic values in the packet. + /// \param[out] acceleration The acceleration values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + void parseVNQMR( + vn::math::vec4f * quaternion, vn::math::vec3f * magnetic, vn::math::vec3f * acceleration, + vn::math::vec3f * angularRate); + +#ifdef INTERNAL + + /// \brief Parses a VNDCM asynchronous packet. + /// + /// \param[out] dcm The directional cosine matrix values in the packet. + void parseVNDCM(math::mat3f * dcm); + +#endif + + /// \brief Parses a VNMAG asynchronous packet. + /// + /// \param[out] magnetic The magnetic values in the packet. + void parseVNMAG(vn::math::vec3f * magnetic); + + /// \brief Parses a VNACC asynchronous packet. + /// + /// \param[out] acceleration The acceleration values in the packet. + void parseVNACC(vn::math::vec3f * acceleration); + + /// \brief Parses a VNGYR asynchronous packet. + /// + /// \param[out] angularRate The angular rate values in the packet. + void parseVNGYR(vn::math::vec3f * angularRate); + + /// \brief Parses a VNMAR asynchronous packet. + /// + /// \param[out] magnetic The magnetic values in the packet. + /// \param[out] acceleration The acceleration values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + void parseVNMAR( + vn::math::vec3f * magnetic, vn::math::vec3f * acceleration, vn::math::vec3f * angularRate); + + /// \brief Parses a VNYMR asynchronous packet. + /// + /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + /// \param[out] magnetic The magnetic values in the packet. + /// \param[out] acceleration The acceleration values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + void parseVNYMR( + vn::math::vec3f * yawPitchRoll, vn::math::vec3f * magnetic, vn::math::vec3f * acceleration, + vn::math::vec3f * angularRate); + +#ifdef INTERNAL + + /// \brief Parses a VNYCM asynchronous packet. + /// + /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + /// \param[out] magnetic The magnetic values in the packet. + /// \param[out] acceleration The acceleration values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + /// \param[out] temperature The temperature value in the packet. + void parseVNYCM( + math::vec3f * yawPitchRoll, math::vec3f * magnetic, math::vec3f * acceleration, + math::vec3f * angularRate, float * temperature); + +#endif + + /// \brief Parses a VNYBA asynchronous packet. + /// + /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + /// \param[out] accelerationBody The acceleration body values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + void parseVNYBA( + vn::math::vec3f * yawPitchRoll, vn::math::vec3f * accelerationBody, + vn::math::vec3f * angularRate); + + /// \brief Parses a VNYIA asynchronous packet. + /// + /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + /// \param[out] accelerationInertial The acceleration inertial values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + void parseVNYIA( + vn::math::vec3f * yawPitchRoll, vn::math::vec3f * accelerationInertial, + vn::math::vec3f * angularRate); + +#ifdef INTERNAL + + /// \brief Parses a VNICM asynchronous packet. + /// + /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + /// \param[out] magnetic The magnetic values in the packet. + /// \param[out] accelerationInertial The acceleration inertial values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + void parseVNICM( + math::vec3f * yawPitchRoll, math::vec3f * magnetic, math::vec3f * accelerationInertial, + math::vec3f * angularRate); + +#endif + + /// \brief Parses a VNIMU asynchronous packet. + /// + /// \param[out] magneticUncompensated The uncompensated magnetic values in the packet. + /// \param[out] accelerationUncompensated The uncompensated acceleration values in the packet. + /// \param[out] angularRateUncompensated The uncompensated angular rate values in the packet. + /// \param[out] temperature The temperature value in the packet. + /// \param[out] pressure The pressure value in the packet. + void parseVNIMU( + vn::math::vec3f * magneticUncompensated, vn::math::vec3f * accelerationUncompensated, + vn::math::vec3f * angularRateUncompensated, float * temperature, float * pressure); + + /// \brief Parses a VNGPS asynchronous packet. + /// + /// \param[out] time The time value in the packet. + /// \param[out] week The week value in the packet. + /// \param[out] gpsFix The GPS fix value in the packet. + /// \param[out] numSats The NumSats value in the packet. + /// \param[out] lla The latitude, longitude and altitude values in the packet. + /// \param[out] nedVel The NED velocity values in the packet. + /// \param[out] nedAcc The NED position accuracy values in the packet. + /// \param[out] speedAcc The SpeedAcc value in the packet. + /// \param[out] timeAcc The TimeAcc value in the packet. + void parseVNGPS( + double * time, uint16_t * week, uint8_t * gpsFix, uint8_t * numSats, vn::math::vec3d * lla, + vn::math::vec3f * nedVel, vn::math::vec3f * nedAcc, float * speedAcc, float * timeAcc); + + /// \brief Parses a VNINS asynchronous packet. + /// + /// \param[out] time The time value in the packet. + /// \param[out] week The week value in the packet. + /// \param[out] status The status value in the packet. + /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + /// \param[out] lla The latitude, longitude, altitude values in the packet. + /// \param[out] nedVel The NED velocity values in the packet. + /// \param[out] attUncertainty The attitude uncertainty value in the packet. + /// \param[out] posUncertainty The position uncertainty value in the packet. + /// \param[out] velUncertainty The velocity uncertainty value in the packet. + void parseVNINS( + double * time, uint16_t * week, uint16_t * status, vn::math::vec3f * yawPitchRoll, + vn::math::vec3d * lla, vn::math::vec3f * nedVel, float * attUncertainty, float * posUncertainty, + float * velUncertainty); + + /// \brief Parses a VNINE asynchronous packet. + /// + /// \param[out] time The time value in the packet. + /// \param[out] week The week value in the packet. + /// \param[out] status The status value in the packet. + /// \param[out] ypr The yaw, pitch, roll values in the packet. + /// \param[out] position The ECEF position values in the packet. + /// \param[out] velocity The ECEF velocity values in the packet. + /// \param[out] attUncertainty The attitude uncertainty value in the packet. + /// \param[out] posUncertainty The position uncertainty value in the packet. + /// \param[out] velUncertainty The velocity uncertainty value in the packet. + void parseVNINE( + double * time, uint16_t * week, uint16_t * status, vn::math::vec3f * ypr, + vn::math::vec3d * position, vn::math::vec3f * velocity, float * attUncertainty, + float * posUncertainty, float * velUncertainty); + + /// \brief Parses a VNISL asynchronous packet. + /// + /// \param[out] ypr The yaw, pitch, roll values in the packet. + /// \param[out] lla The latitude, longitude, altitude values in the packet. + /// \param[out] velocity The velocity values in the packet. + /// \param[out] acceleration The acceleration values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + void parseVNISL( + vn::math::vec3f * ypr, vn::math::vec3d * lla, vn::math::vec3f * velocity, + vn::math::vec3f * acceleration, vn::math::vec3f * angularRate); + + /// \brief Parses a VNISE asynchronous packet. + /// + /// \param[out] ypr The yaw, pitch, roll values in the packet. + /// \param[out] position The ECEF position values in the packet. + /// \param[out] velocity The ECEF velocity values in the packet. + /// \param[out] acceleration The acceleration values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + void parseVNISE( + vn::math::vec3f * ypr, vn::math::vec3d * position, vn::math::vec3f * velocity, + vn::math::vec3f * acceleration, vn::math::vec3f * angularRate); + +#ifdef INTERNAL + + /// \brief Parses a VNRAW asynchronous packet. + /// + /// \param[out] magneticVoltage The magnetic voltage values in the packet. + /// \param[out] accelerationVoltage The acceleration voltage values in the packet. + /// \param[out] angularRateVoltage The angular rate voltage values in the packet. + /// \param[out] temperatureVoltage The temperature voltage value in the packet. + void parseVNRAW( + math::vec3f * magneticVoltage, math::vec3f * accelerationVoltage, + math::vec3f * angularRateVoltage, float * temperatureVoltage); + + /// \brief Parses a VNCMV asynchronous packet. + /// + /// \param[out] magneticUncompensated The uncompensated magnetic values in the packet. + /// \param[out] accelerationUncompensated The uncompensated acceleration values in the packet. + /// \param[out] angularRateUncompensated The uncompensated angular rate values in the packet. + /// \param[out] temperature The temperature value in the packet. + void parseVNCMV( + math::vec3f * magneticUncompensated, math::vec3f * accelerationUncompensated, + math::vec3f * angularRateUncompensated, float * temperature); + + /// \brief Parses a VNSTV asynchronous packet. + /// + /// \param[out] quaternion The quaternion values in the packet. + /// \param[out] angularRateBias The angular rate bias values in the packet. + void parseVNSTV(math::vec4f * quaternion, math::vec3f * angularRateBias); + + /// \brief Parses a VNCOV asynchronous packet. + /// + /// \param[out] attitudeVariance The attitude variance values in the packet. + /// \param[out] angularRateBiasVariance The angular rate bias variance values in the packet. + void parseVNCOV(math::vec3f * attitudeVariance, math::vec3f * angularRateBiasVariance); + +#endif + + /// \brief Parses a VNGPE asynchronous packet. + /// + /// \param[out] tow The tow value in the packet. + /// \param[out] week The week value in the packet. + /// \param[out] gpsFix The GPS fix value in the packet. + /// \param[out] numSats The numSats value in the packet. + /// \param[out] position The ECEF position values in the packet. + /// \param[out] velocity The ECEF velocity values in the packet. + /// \param[out] posAcc The PosAcc values in the packet. + /// \param[out] speedAcc The SpeedAcc value in the packet. + /// \param[out] timeAcc The TimeAcc value in the packet. + void parseVNGPE( + double * tow, uint16_t * week, uint8_t * gpsFix, uint8_t * numSats, vn::math::vec3d * position, + vn::math::vec3f * velocity, vn::math::vec3f * posAcc, float * speedAcc, float * timeAcc); + + /// \brief Parses a VNDTV asynchronous packet. + /// + /// \param[out] deltaTime The DeltaTime value in the packet. + /// \param[out] deltaTheta The DeltaTheta values in the packet. + /// \param[out] deltaVelocity The DeltaVelocity values in the packet. + void parseVNDTV(float * deltaTime, vn::math::vec3f * deltaTheta, vn::math::vec3f * deltaVelocity); + + /// \} + + /// \defgroup uartAsciiResponseParsers UART ASCII Response Packet Parsers + /// \brief This group of methods allow parsing of ASCII response data + /// packets from VectorNav's sensors. + /// + /// The units are not specified for the out parameters since these + /// methods do a simple conversion operation from the packet string. Please + /// consult the appropriate user manual for your sensor for details about + /// the units returned by the sensor. + /// + /// \{ + + /// \brief Parses a response from reading any of the Binary Output registers. + /// + /// \param[out] asyncMode The register's AsyncMode field. + /// \param[out] rateDivisor The register's RateDivisor field. + /// \param[out] outputGroup The register's OutputGroup field. + /// \param[out] commonField The set fields of Output Group 1 (Common) if present. + /// \param[out] timeField The set fields of Output Group 2 (Time) if present. + /// \param[out] imuField The set fields of Output Group 3 (IMU) if present. + /// \param[out] gpsField The set fields of Output Group 4 (GPS) if present. + /// \param[out] attitudeField The set fields of Output Group 5 (Attitude) if present. /// \param[out] insField The set fields of Output Group 6 (INS) if present. /// \param[out] gps2Field The set fields of Output Group 7 (GPS2) if present. void parseBinaryOutput( - uint16_t* asyncMode, - uint16_t* rateDivisor, - uint16_t* outputGroup, - uint16_t* commonField, - uint16_t* timeField, - uint16_t* imuField, - uint16_t* gpsField, - uint16_t* attitudeField, - uint16_t* insField, - uint16_t* gps2Field); - - /// \brief Parses a response from reading the User Tag register. - /// - /// \param[out] tag The register's Tag field. - void parseUserTag(char* tag); - - /// \brief Parses a response from reading the Model Number register. - /// - /// \param[out] productName The register's Product Name field. - void parseModelNumber(char* productName); - - /// \brief Parses a response from reading the Hardware Revision register. - /// - /// \param[out] revision The register's Revision field. - void parseHardwareRevision(uint32_t* revision); - - /// \brief Parses a response from reading the Serial Number register. - /// - /// \param[out] serialNum The register's SerialNum field. - void parseSerialNumber(uint32_t* serialNum); - - /// \brief Parses a response from reading the Firmware Version register. - /// - /// \param[out] firmwareVersion The register's Firmware Version field. - void parseFirmwareVersion(char* firmwareVersion); - - /// \brief Parses a response from reading the Serial Baud Rate register. - /// - /// \param[out] baudrate The register's Baud Rate field. - void parseSerialBaudRate(uint32_t* baudrate); - - /// \brief Parses a response from reading the Async Data Output Type register. - /// - /// \param[out] ador The register's ADOR field. - void parseAsyncDataOutputType(uint32_t* ador); - - /// \brief Parses a response from reading the Async Data Output Frequency register. - /// - /// \param[out] adof The register's ADOF field. - void parseAsyncDataOutputFrequency(uint32_t* adof); - - /// \brief Parses a response from reading the Yaw Pitch Roll register. - /// - /// \param[out] yawPitchRoll The register's YawPitchRoll field. - void parseYawPitchRoll(vn::math::vec3f* yawPitchRoll); - - /// \brief Parses a response from reading the Attitude Quaternion register. - /// - /// \param[out] quat The register's Quat field. - void parseAttitudeQuaternion(vn::math::vec4f* quat); - - /// \brief Parses a response from reading the Quaternion, Magnetic, Acceleration and Angular Rates register. - /// - /// \param[out] quat The register's Quat field. - /// \param[out] mag The register's Mag field. - /// \param[out] accel The register's Accel field. - /// \param[out] gyro The register's Gyro field. - void parseQuaternionMagneticAccelerationAndAngularRates(vn::math::vec4f* quat, vn::math::vec3f* mag, vn::math::vec3f* accel, vn::math::vec3f* gyro); - - /// \brief Parses a response from reading the Magnetic Measurements register. - /// - /// \param[out] mag The register's Mag field. - void parseMagneticMeasurements(vn::math::vec3f* mag); - - /// \brief Parses a response from reading the Acceleration Measurements register. - /// - /// \param[out] accel The register's Accel field. - void parseAccelerationMeasurements(vn::math::vec3f* accel); - - /// \brief Parses a response from reading the Angular Rate Measurements register. - /// - /// \param[out] gyro The register's Gyro field. - void parseAngularRateMeasurements(vn::math::vec3f* gyro); - - /// \brief Parses a response from reading the Magnetic, Acceleration and Angular Rates register. - /// - /// \param[out] mag The register's Mag field. - /// \param[out] accel The register's Accel field. - /// \param[out] gyro The register's Gyro field. - void parseMagneticAccelerationAndAngularRates(vn::math::vec3f* mag, vn::math::vec3f* accel, vn::math::vec3f* gyro); - - /// \brief Parses a response from reading the Magnetic and Gravity Reference Vectors register. - /// - /// \param[out] magRef The register's MagRef field. - /// \param[out] accRef The register's AccRef field. - void parseMagneticAndGravityReferenceVectors(vn::math::vec3f* magRef, vn::math::vec3f* accRef); - - /// \brief Parses a response from reading the Filter Measurements Variance Parameters register. - /// - /// \param[out] angularWalkVariance The register's Angular Walk Variance field. - /// \param[out] angularRateVariance The register's Angular Rate Variance field. - /// \param[out] magneticVariance The register's Magnetic Variance field. - /// \param[out] accelerationVariance The register's Acceleration Variance field. - void parseFilterMeasurementsVarianceParameters(float* angularWalkVariance, vn::math::vec3f* angularRateVariance, vn::math::vec3f* magneticVariance, vn::math::vec3f* accelerationVariance); - - /// \brief Parses a response from reading the Magnetometer Compensation register. - /// - /// \param[out] c The register's C field. - /// \param[out] b The register's B field. - void parseMagnetometerCompensation(vn::math::mat3f* c, vn::math::vec3f* b); - - /// \brief Parses a response from reading the Filter Active Tuning Parameters register. - /// - /// \param[out] magneticDisturbanceGain The register's Magnetic Disturbance Gain field. - /// \param[out] accelerationDisturbanceGain The register's Acceleration Disturbance Gain field. - /// \param[out] magneticDisturbanceMemory The register's Magnetic Disturbance Memory field. - /// \param[out] accelerationDisturbanceMemory The register's Acceleration Disturbance Memory field. - void parseFilterActiveTuningParameters(float* magneticDisturbanceGain, float* accelerationDisturbanceGain, float* magneticDisturbanceMemory, float* accelerationDisturbanceMemory); - - /// \brief Parses a response from reading the Acceleration Compensation register. - /// - /// \param[out] c The register's C field. - /// \param[out] b The register's B field. - void parseAccelerationCompensation(vn::math::mat3f* c, vn::math::vec3f* b); - - /// \brief Parses a response from reading the Reference Frame Rotation register. - /// - /// \param[out] c The register's C field. - void parseReferenceFrameRotation(vn::math::mat3f* c); - - /// \brief Parses a response from reading the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register. - /// - /// \param[out] yawPitchRoll The register's YawPitchRoll field. - /// \param[out] mag The register's Mag field. - /// \param[out] accel The register's Accel field. - /// \param[out] gyro The register's Gyro field. - void parseYawPitchRollMagneticAccelerationAndAngularRates(vn::math::vec3f* yawPitchRoll, vn::math::vec3f* mag, vn::math::vec3f* accel, vn::math::vec3f* gyro); - - /// \brief Parses a response from reading the Communication Protocol Control register. - /// - /// \param[out] serialCount The register's SerialCount field. - /// \param[out] serialStatus The register's SerialStatus field. - /// \param[out] spiCount The register's SPICount field. - /// \param[out] spiStatus The register's SPIStatus field. - /// \param[out] serialChecksum The register's SerialChecksum field. - /// \param[out] spiChecksum The register's SPIChecksum field. - /// \param[out] errorMode The register's ErrorMode field. - void parseCommunicationProtocolControl(uint8_t* serialCount, uint8_t* serialStatus, uint8_t* spiCount, uint8_t* spiStatus, uint8_t* serialChecksum, uint8_t* spiChecksum, uint8_t* errorMode); - - /// \brief Parses a response from reading the Synchronization Control register. - /// - /// \param[out] syncInMode The register's SyncInMode field. - /// \param[out] syncInEdge The register's SyncInEdge field. - /// \param[out] syncInSkipFactor The register's SyncInSkipFactor field. - /// \param[out] syncOutMode The register's SyncOutMode field. - /// \param[out] syncOutPolarity The register's SyncOutPolarity field. - /// \param[out] syncOutSkipFactor The register's SyncOutSkipFactor field. - /// \param[out] syncOutPulseWidth The register's SyncOutPulseWidth field. - void parseSynchronizationControl(uint8_t* syncInMode, uint8_t* syncInEdge, uint16_t* syncInSkipFactor, uint8_t* syncOutMode, uint8_t* syncOutPolarity, uint16_t* syncOutSkipFactor, uint32_t* syncOutPulseWidth); - - /// \brief Parses a response from reading the Synchronization Status register. - /// - /// \param[out] syncInCount The register's SyncInCount field. - /// \param[out] syncInTime The register's SyncInTime field. - /// \param[out] syncOutCount The register's SyncOutCount field. - void parseSynchronizationStatus(uint32_t* syncInCount, uint32_t* syncInTime, uint32_t* syncOutCount); - - /// \brief Parses a response from reading the Filter Basic Control register. - /// - /// \param[out] magMode The register's MagMode field. - /// \param[out] extMagMode The register's ExtMagMode field. - /// \param[out] extAccMode The register's ExtAccMode field. - /// \param[out] extGyroMode The register's ExtGyroMode field. - /// \param[out] gyroLimit The register's GyroLimit field. - void parseFilterBasicControl(uint8_t* magMode, uint8_t* extMagMode, uint8_t* extAccMode, uint8_t* extGyroMode, vn::math::vec3f* gyroLimit); - - // Added by jesperh 2021-02-09 - void parseHeaveConfiguration( - float* initialWavePeriod, - float* initialWaveAmplitude, - float* maxWavePeriod, - float* minWaveAmplitude, - float* delayedHeaveCutoffFreq, - float* heaveCutoffFreq, - float* heaveRateCutoffFreq); - /// \brief Parses a response from reading the VPE Basic Control register. - /// - /// \param[out] enable The register's Enable field. - /// \param[out] headingMode The register's HeadingMode field. - /// \param[out] filteringMode The register's FilteringMode field. - /// \param[out] tuningMode The register's TuningMode field. - void parseVpeBasicControl(uint8_t* enable, uint8_t* headingMode, uint8_t* filteringMode, uint8_t* tuningMode); - - /// \brief Parses a response from reading the VPE Magnetometer Basic Tuning register. - /// - /// \param[out] baseTuning The register's BaseTuning field. - /// \param[out] adaptiveTuning The register's AdaptiveTuning field. - /// \param[out] adaptiveFiltering The register's AdaptiveFiltering field. - void parseVpeMagnetometerBasicTuning(vn::math::vec3f* baseTuning, vn::math::vec3f* adaptiveTuning, vn::math::vec3f* adaptiveFiltering); - - /// \brief Parses a response from reading the VPE Magnetometer Advanced Tuning register. - /// - /// \param[out] minFiltering The register's MinFiltering field. - /// \param[out] maxFiltering The register's MaxFiltering field. - /// \param[out] maxAdaptRate The register's MaxAdaptRate field. - /// \param[out] disturbanceWindow The register's DisturbanceWindow field. - /// \param[out] maxTuning The register's MaxTuning field. - void parseVpeMagnetometerAdvancedTuning(vn::math::vec3f* minFiltering, vn::math::vec3f* maxFiltering, float* maxAdaptRate, float* disturbanceWindow, float* maxTuning); - - /// \brief Parses a response from reading the VPE Accelerometer Basic Tuning register. - /// - /// \param[out] baseTuning The register's BaseTuning field. - /// \param[out] adaptiveTuning The register's AdaptiveTuning field. - /// \param[out] adaptiveFiltering The register's AdaptiveFiltering field. - void parseVpeAccelerometerBasicTuning(vn::math::vec3f* baseTuning, vn::math::vec3f* adaptiveTuning, vn::math::vec3f* adaptiveFiltering); - - /// \brief Parses a response from reading the VPE Accelerometer Advanced Tuning register. - /// - /// \param[out] minFiltering The register's MinFiltering field. - /// \param[out] maxFiltering The register's MaxFiltering field. - /// \param[out] maxAdaptRate The register's MaxAdaptRate field. - /// \param[out] disturbanceWindow The register's DisturbanceWindow field. - /// \param[out] maxTuning The register's MaxTuning field. - void parseVpeAccelerometerAdvancedTuning(vn::math::vec3f* minFiltering, vn::math::vec3f* maxFiltering, float* maxAdaptRate, float* disturbanceWindow, float* maxTuning); - - /// \brief Parses a response from reading the VPE Gyro Basic Tuning register. - /// - /// \param[out] angularWalkVariance The register's AngularWalkVariance field. - /// \param[out] baseTuning The register's BaseTuning field. - /// \param[out] adaptiveTuning The register's AdaptiveTuning field. - void parseVpeGyroBasicTuning(vn::math::vec3f* angularWalkVariance, vn::math::vec3f* baseTuning, vn::math::vec3f* adaptiveTuning); - - /// \brief Parses a response from reading the Filter Startup Gyro Bias register. - /// - /// \param[out] bias The register's Bias field. - void parseFilterStartupGyroBias(vn::math::vec3f* bias); - - /// \brief Parses a response from reading the Magnetometer Calibration Control register. - /// - /// \param[out] hsiMode The register's HSIMode field. - /// \param[out] hsiOutput The register's HSIOutput field. - /// \param[out] convergeRate The register's ConvergeRate field. - void parseMagnetometerCalibrationControl(uint8_t* hsiMode, uint8_t* hsiOutput, uint8_t* convergeRate); - - /// \brief Parses a response from reading the Calculated Magnetometer Calibration register. - /// - /// \param[out] c The register's C field. - /// \param[out] b The register's B field. - void parseCalculatedMagnetometerCalibration(vn::math::mat3f* c, vn::math::vec3f* b); - - /// \brief Parses a response from reading the Indoor Heading Mode Control register. - /// - /// \param[out] maxRateError The register's Max Rate Error field. - void parseIndoorHeadingModeControl(float* maxRateError); - - /// \brief Parses a response from reading the Velocity Compensation Measurement register. - /// - /// \param[out] velocity The register's Velocity field. - void parseVelocityCompensationMeasurement(vn::math::vec3f* velocity); - - /// \brief Parses a response from reading the Velocity Compensation Control register. - /// - /// \param[out] mode The register's Mode field. - /// \param[out] velocityTuning The register's VelocityTuning field. - /// \param[out] rateTuning The register's RateTuning field. - void parseVelocityCompensationControl(uint8_t* mode, float* velocityTuning, float* rateTuning); - - /// \brief Parses a response from reading the Velocity Compensation Status register. - /// - /// \param[out] x The register's x field. - /// \param[out] xDot The register's xDot field. - /// \param[out] accelOffset The register's accelOffset field. - /// \param[out] omega The register's omega field. - void parseVelocityCompensationStatus(float* x, float* xDot, vn::math::vec3f* accelOffset, vn::math::vec3f* omega); - - /// \brief Parses a response from reading the IMU Measurements register. - /// - /// \param[out] mag The register's Mag field. - /// \param[out] accel The register's Accel field. - /// \param[out] gyro The register's Gyro field. - /// \param[out] temp The register's Temp field. - /// \param[out] pressure The register's Pressure field. - void parseImuMeasurements(vn::math::vec3f* mag, vn::math::vec3f* accel, vn::math::vec3f* gyro, float* temp, float* pressure); - - /// \brief Parses a response from reading the GPS Configuration register (deprecated). - /// - /// \param[out] mode The register's Mode field. - /// \param[out] ppsSource The register's PpsSource field. - void parseGpsConfiguration(uint8_t* mode, uint8_t* ppsSource); - - /// \brief Parses a response from reading the GPS Configuration register. - /// - /// \param[out] mode The register's Mode field. - /// \param[out] ppsSource The register's PpsSource field. - /// \param[out] rate The register's Rate field. - /// \param[out] antPow The register's AntPower field. - void parseGpsConfiguration(uint8_t* mode, uint8_t* ppsSource, uint8_t* rate, uint8_t* antPow); - - /// \brief Parses a response from reading the GPS Antenna Offset register. - /// - /// \param[out] position The register's Position field. - void parseGpsAntennaOffset(vn::math::vec3f* position); - - /// \brief Parses a response from reading the GPS Solution - LLA register. - /// - /// \param[out] time The register's Time field. - /// \param[out] week The register's Week field. - /// \param[out] gpsFix The register's GpsFix field. - /// \param[out] numSats The register's NumSats field. - /// \param[out] lla The register's Lla field. - /// \param[out] nedVel The register's NedVel field. - /// \param[out] nedAcc The register's NedAcc field. - /// \param[out] speedAcc The register's SpeedAcc field. - /// \param[out] timeAcc The register's TimeAcc field. - void parseGpsSolutionLla(double* time, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, vn::math::vec3d* lla, vn::math::vec3f* nedVel, vn::math::vec3f* nedAcc, float* speedAcc, float* timeAcc); - - /// \brief Parses a response from reading the GPS Solution - ECEF register. - /// - /// \param[out] tow The register's Tow field. - /// \param[out] week The register's Week field. - /// \param[out] gpsFix The register's GpsFix field. - /// \param[out] numSats The register's NumSats field. - /// \param[out] position The register's Position field. - /// \param[out] velocity The register's Velocity field. - /// \param[out] posAcc The register's PosAcc field. - /// \param[out] speedAcc The register's SpeedAcc field. - /// \param[out] timeAcc The register's TimeAcc field. - void parseGpsSolutionEcef(double* tow, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, vn::math::vec3d* position, vn::math::vec3f* velocity, vn::math::vec3f* posAcc, float* speedAcc, float* timeAcc); - - /// \brief Parses a response from reading the INS Solution - LLA register. - /// - /// \param[out] time The register's Time field. - /// \param[out] week The register's Week field. - /// \param[out] status The register's Status field. - /// \param[out] yawPitchRoll The register's YawPitchRoll field. - /// \param[out] position The register's Position field. - /// \param[out] nedVel The register's NedVel field. - /// \param[out] attUncertainty The register's AttUncertainty field. - /// \param[out] posUncertainty The register's PosUncertainty field. - /// \param[out] velUncertainty The register's VelUncertainty field. - void parseInsSolutionLla(double* time, uint16_t* week, uint16_t* status, vn::math::vec3f* yawPitchRoll, vn::math::vec3d* position, vn::math::vec3f* nedVel, float* attUncertainty, float* posUncertainty, float* velUncertainty); - - /// \brief Parses a response from reading the INS Solution - ECEF register. - /// - /// \param[out] time The register's Time field. - /// \param[out] week The register's Week field. - /// \param[out] status The register's Status field. - /// \param[out] yawPitchRoll The register's YawPitchRoll field. - /// \param[out] position The register's Position field. - /// \param[out] velocity The register's Velocity field. - /// \param[out] attUncertainty The register's AttUncertainty field. - /// \param[out] posUncertainty The register's PosUncertainty field. - /// \param[out] velUncertainty The register's VelUncertainty field. - void parseInsSolutionEcef(double* time, uint16_t* week, uint16_t* status, vn::math::vec3f* yawPitchRoll, vn::math::vec3d* position, vn::math::vec3f* velocity, float* attUncertainty, float* posUncertainty, float* velUncertainty); - - /// \brief Parses a response from reading the INS Basic Configuration register. - /// - /// \param[out] scenario The register's Scenario field. - /// \param[out] ahrsAiding The register's AhrsAiding field. - void parseInsBasicConfiguration(uint8_t* scenario, uint8_t* ahrsAiding); - - /// \brief Parses a response from reading the INS Basic Configuration register. - /// - /// \param[out] scenario The register's Scenario field. - /// \param[out] ahrsAiding The register's AhrsAiding field. - /// \param[out] estBaseline The register's EstBaseline field. - void parseInsBasicConfiguration(uint8_t* scenario, uint8_t* ahrsAiding, uint8_t* estBaseline); - - /// \brief Parses a response from reading the INS Advanced Configuration register. - /// - /// \param[out] useMag The register's UseMag field. - /// \param[out] usePres The register's UsePres field. - /// \param[out] posAtt The register's PosAtt field. - /// \param[out] velAtt The register's VelAtt field. - /// \param[out] velBias The register's VelBias field. - /// \param[out] useFoam The register's UseFoam field. - /// \param[out] gpsCovType The register's GPSCovType field. - /// \param[out] velCount The register's VelCount field. - /// \param[out] velInit The register's VelInit field. - /// \param[out] moveOrigin The register's MoveOrigin field. - /// \param[out] gpsTimeout The register's GPSTimeout field. - /// \param[out] deltaLimitPos The register's DeltaLimitPos field. - /// \param[out] deltaLimitVel The register's DeltaLimitVel field. - /// \param[out] minPosUncertainty The register's MinPosUncertainty field. - /// \param[out] minVelUncertainty The register's MinVelUncertainty field. - void parseInsAdvancedConfiguration(uint8_t* useMag, uint8_t* usePres, uint8_t* posAtt, uint8_t* velAtt, uint8_t* velBias, uint8_t* useFoam, uint8_t* gpsCovType, uint8_t* velCount, float* velInit, float* moveOrigin, float* gpsTimeout, float* deltaLimitPos, float* deltaLimitVel, float* minPosUncertainty, float* minVelUncertainty); - - /// \brief Parses a response from reading the INS State - LLA register. - /// - /// \param[out] yawPitchRoll The register's YawPitchRoll field. - /// \param[out] position The register's Position field. - /// \param[out] velocity The register's Velocity field. - /// \param[out] accel The register's Accel field. - /// \param[out] angularRate The register's AngularRate field. - void parseInsStateLla(vn::math::vec3f* yawPitchRoll, vn::math::vec3d* position, vn::math::vec3f* velocity, vn::math::vec3f* accel, vn::math::vec3f* angularRate); - - /// \brief Parses a response from reading the INS State - ECEF register. - /// - /// \param[out] yawPitchRoll The register's YawPitchRoll field. - /// \param[out] position The register's Position field. - /// \param[out] velocity The register's Velocity field. - /// \param[out] accel The register's Accel field. - /// \param[out] angularRate The register's AngularRate field. - void parseInsStateEcef(vn::math::vec3f* yawPitchRoll, vn::math::vec3d* position, vn::math::vec3f* velocity, vn::math::vec3f* accel, vn::math::vec3f* angularRate); - - /// \brief Parses a response from reading the Startup Filter Bias Estimate register. - /// - /// \param[out] gyroBias The register's GyroBias field. - /// \param[out] accelBias The register's AccelBias field. - /// \param[out] pressureBias The register's PressureBias field. - void parseStartupFilterBiasEstimate(vn::math::vec3f* gyroBias, vn::math::vec3f* accelBias, float* pressureBias); - - /// \brief Parses a response from reading the Delta Theta and Delta Velocity register. - /// - /// \param[out] deltaTime The register's DeltaTime field. - /// \param[out] deltaTheta The register's DeltaTheta field. - /// \param[out] deltaVelocity The register's DeltaVelocity field. - void parseDeltaThetaAndDeltaVelocity(float* deltaTime, vn::math::vec3f* deltaTheta, vn::math::vec3f* deltaVelocity); - - /// \brief Parses a response from reading the Delta Theta and Delta Velocity Configuration register (deprecated). - /// - /// \param[out] integrationFrame The register's IntegrationFrame field. - /// \param[out] gyroCompensation The register's GyroCompensation field. - /// \param[out] accelCompensation The register's AccelCompensation field. - void parseDeltaThetaAndDeltaVelocityConfiguration(uint8_t* integrationFrame, uint8_t* gyroCompensation, uint8_t* accelCompensation); - - /// \brief Parses a response from reading the Delta Theta and Delta Velocity Configuration register. - /// - /// \param[out] integrationFrame The register's IntegrationFrame field. - /// \param[out] gyroCompensation The register's GyroCompensation field. - /// \param[out] accelCompensation The register's AccelCompensation field. - /// \param[out] earthRateCorrection The register's EarthRateCorrection field. - void parseDeltaThetaAndDeltaVelocityConfiguration(uint8_t* integrationFrame, uint8_t* gyroCompensation, uint8_t* accelCompensation, uint8_t* earthRateCorrection); - - /// \brief Parses a response from reading the Reference Vector Configuration register. - /// - /// \param[out] useMagModel The register's UseMagModel field. - /// \param[out] useGravityModel The register's UseGravityModel field. - /// \param[out] recalcThreshold The register's RecalcThreshold field. - /// \param[out] year The register's Year field. - /// \param[out] position The register's Position field. - void parseReferenceVectorConfiguration(uint8_t* useMagModel, uint8_t* useGravityModel, uint32_t* recalcThreshold, float* year, vn::math::vec3d* position); - - /// \brief Parses a response from reading the Gyro Compensation register. - /// - /// \param[out] c The register's C field. - /// \param[out] b The register's B field. - void parseGyroCompensation(vn::math::mat3f* c, vn::math::vec3f* b); - - /// \brief Parses a response from reading the IMU Filtering Configuration register. - /// - /// \param[out] magWindowSize The register's MagWindowSize field. - /// \param[out] accelWindowSize The register's AccelWindowSize field. - /// \param[out] gyroWindowSize The register's GyroWindowSize field. - /// \param[out] tempWindowSize The register's TempWindowSize field. - /// \param[out] presWindowSize The register's PresWindowSize field. - /// \param[out] magFilterMode The register's MagFilterMode field. - /// \param[out] accelFilterMode The register's AccelFilterMode field. - /// \param[out] gyroFilterMode The register's GyroFilterMode field. - /// \param[out] tempFilterMode The register's TempFilterMode field. - /// \param[out] presFilterMode The register's PresFilterMode field. - void parseImuFilteringConfiguration(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 Parses a response from reading the GPS Compass Baseline register. - /// - /// \param[out] position The register's Position field. - /// \param[out] uncertainty The register's Uncertainty field. - void parseGpsCompassBaseline(vn::math::vec3f* position, vn::math::vec3f* uncertainty); - - /// \brief Parses a response from reading the GPS Compass Estimated Baseline register. - /// - /// \param[out] estBaselineUsed The register's EstBaselineUsed field. - /// \param[out] numMeas The register's NumMeas field. - /// \param[out] position The register's Position field. - /// \param[out] uncertainty The register's Uncertainty field. - void parseGpsCompassEstimatedBaseline(uint8_t* estBaselineUsed, uint16_t* numMeas, vn::math::vec3f* position, vn::math::vec3f* uncertainty); - - /// \brief Parses a response from reading the IMU Rate Configuration register. - /// - /// \param[out] imuRate The register's imuRate field. - /// \param[out] navDivisor The register's NavDivisor field. - /// \param[out] filterTargetRate The register's filterTargetRate field. - /// \param[out] filterMinRate The register's filterMinRate field. - void parseImuRateConfiguration(uint16_t* imuRate, uint16_t* navDivisor, float* filterTargetRate, float* filterMinRate); - - /// \brief Parses a response from reading the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register. - /// - /// \param[out] yawPitchRoll The register's YawPitchRoll field. - /// \param[out] bodyAccel The register's BodyAccel field. - /// \param[out] gyro The register's Gyro field. - void parseYawPitchRollTrueBodyAccelerationAndAngularRates(vn::math::vec3f* yawPitchRoll, vn::math::vec3f* bodyAccel, vn::math::vec3f* gyro); - - /// \brief Parses a response from reading the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register. - /// - /// \param[out] yawPitchRoll The register's YawPitchRoll field. - /// \param[out] inertialAccel The register's InertialAccel field. - /// \param[out] gyro The register's Gyro field. - void parseYawPitchRollTrueInertialAccelerationAndAngularRates(vn::math::vec3f* yawPitchRoll, vn::math::vec3f* inertialAccel, vn::math::vec3f* gyro); - - /// \} + uint16_t * asyncMode, uint16_t * rateDivisor, uint16_t * outputGroup, uint16_t * commonField, + uint16_t * timeField, uint16_t * imuField, uint16_t * gpsField, uint16_t * attitudeField, + uint16_t * insField, uint16_t * gps2Field); + + /// \brief Parses a response from reading the User Tag register. + /// + /// \param[out] tag The register's Tag field. + void parseUserTag(char * tag); + + /// \brief Parses a response from reading the Model Number register. + /// + /// \param[out] productName The register's Product Name field. + void parseModelNumber(char * productName); + + /// \brief Parses a response from reading the Hardware Revision register. + /// + /// \param[out] revision The register's Revision field. + void parseHardwareRevision(uint32_t * revision); + + /// \brief Parses a response from reading the Serial Number register. + /// + /// \param[out] serialNum The register's SerialNum field. + void parseSerialNumber(uint32_t * serialNum); + + /// \brief Parses a response from reading the Firmware Version register. + /// + /// \param[out] firmwareVersion The register's Firmware Version field. + void parseFirmwareVersion(char * firmwareVersion); + + /// \brief Parses a response from reading the Serial Baud Rate register. + /// + /// \param[out] baudrate The register's Baud Rate field. + void parseSerialBaudRate(uint32_t * baudrate); + + /// \brief Parses a response from reading the Async Data Output Type register. + /// + /// \param[out] ador The register's ADOR field. + void parseAsyncDataOutputType(uint32_t * ador); + + /// \brief Parses a response from reading the Async Data Output Frequency register. + /// + /// \param[out] adof The register's ADOF field. + void parseAsyncDataOutputFrequency(uint32_t * adof); + + /// \brief Parses a response from reading the Yaw Pitch Roll register. + /// + /// \param[out] yawPitchRoll The register's YawPitchRoll field. + void parseYawPitchRoll(vn::math::vec3f * yawPitchRoll); + + /// \brief Parses a response from reading the Attitude Quaternion register. + /// + /// \param[out] quat The register's Quat field. + void parseAttitudeQuaternion(vn::math::vec4f * quat); + + /// \brief Parses a response from reading the Quaternion, Magnetic, Acceleration and Angular Rates register. + /// + /// \param[out] quat The register's Quat field. + /// \param[out] mag The register's Mag field. + /// \param[out] accel The register's Accel field. + /// \param[out] gyro The register's Gyro field. + void parseQuaternionMagneticAccelerationAndAngularRates( + vn::math::vec4f * quat, vn::math::vec3f * mag, vn::math::vec3f * accel, vn::math::vec3f * gyro); + + /// \brief Parses a response from reading the Magnetic Measurements register. + /// + /// \param[out] mag The register's Mag field. + void parseMagneticMeasurements(vn::math::vec3f * mag); + + /// \brief Parses a response from reading the Acceleration Measurements register. + /// + /// \param[out] accel The register's Accel field. + void parseAccelerationMeasurements(vn::math::vec3f * accel); + + /// \brief Parses a response from reading the Angular Rate Measurements register. + /// + /// \param[out] gyro The register's Gyro field. + void parseAngularRateMeasurements(vn::math::vec3f * gyro); + + /// \brief Parses a response from reading the Magnetic, Acceleration and Angular Rates register. + /// + /// \param[out] mag The register's Mag field. + /// \param[out] accel The register's Accel field. + /// \param[out] gyro The register's Gyro field. + void parseMagneticAccelerationAndAngularRates( + vn::math::vec3f * mag, vn::math::vec3f * accel, vn::math::vec3f * gyro); + + /// \brief Parses a response from reading the Magnetic and Gravity Reference Vectors register. + /// + /// \param[out] magRef The register's MagRef field. + /// \param[out] accRef The register's AccRef field. + void parseMagneticAndGravityReferenceVectors(vn::math::vec3f * magRef, vn::math::vec3f * accRef); + + /// \brief Parses a response from reading the Filter Measurements Variance Parameters register. + /// + /// \param[out] angularWalkVariance The register's Angular Walk Variance field. + /// \param[out] angularRateVariance The register's Angular Rate Variance field. + /// \param[out] magneticVariance The register's Magnetic Variance field. + /// \param[out] accelerationVariance The register's Acceleration Variance field. + void parseFilterMeasurementsVarianceParameters( + float * angularWalkVariance, vn::math::vec3f * angularRateVariance, + vn::math::vec3f * magneticVariance, vn::math::vec3f * accelerationVariance); + + /// \brief Parses a response from reading the Magnetometer Compensation register. + /// + /// \param[out] c The register's C field. + /// \param[out] b The register's B field. + void parseMagnetometerCompensation(vn::math::mat3f * c, vn::math::vec3f * b); + + /// \brief Parses a response from reading the Filter Active Tuning Parameters register. + /// + /// \param[out] magneticDisturbanceGain The register's Magnetic Disturbance Gain field. + /// \param[out] accelerationDisturbanceGain The register's Acceleration Disturbance Gain field. + /// \param[out] magneticDisturbanceMemory The register's Magnetic Disturbance Memory field. + /// \param[out] accelerationDisturbanceMemory The register's Acceleration Disturbance Memory field. + void parseFilterActiveTuningParameters( + float * magneticDisturbanceGain, float * accelerationDisturbanceGain, + float * magneticDisturbanceMemory, float * accelerationDisturbanceMemory); + + /// \brief Parses a response from reading the Acceleration Compensation register. + /// + /// \param[out] c The register's C field. + /// \param[out] b The register's B field. + void parseAccelerationCompensation(vn::math::mat3f * c, vn::math::vec3f * b); + + /// \brief Parses a response from reading the Reference Frame Rotation register. + /// + /// \param[out] c The register's C field. + void parseReferenceFrameRotation(vn::math::mat3f * c); + + /// \brief Parses a response from reading the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register. + /// + /// \param[out] yawPitchRoll The register's YawPitchRoll field. + /// \param[out] mag The register's Mag field. + /// \param[out] accel The register's Accel field. + /// \param[out] gyro The register's Gyro field. + void parseYawPitchRollMagneticAccelerationAndAngularRates( + vn::math::vec3f * yawPitchRoll, vn::math::vec3f * mag, vn::math::vec3f * accel, + vn::math::vec3f * gyro); + + /// \brief Parses a response from reading the Communication Protocol Control register. + /// + /// \param[out] serialCount The register's SerialCount field. + /// \param[out] serialStatus The register's SerialStatus field. + /// \param[out] spiCount The register's SPICount field. + /// \param[out] spiStatus The register's SPIStatus field. + /// \param[out] serialChecksum The register's SerialChecksum field. + /// \param[out] spiChecksum The register's SPIChecksum field. + /// \param[out] errorMode The register's ErrorMode field. + void parseCommunicationProtocolControl( + uint8_t * serialCount, uint8_t * serialStatus, uint8_t * spiCount, uint8_t * spiStatus, + uint8_t * serialChecksum, uint8_t * spiChecksum, uint8_t * errorMode); + + /// \brief Parses a response from reading the Synchronization Control register. + /// + /// \param[out] syncInMode The register's SyncInMode field. + /// \param[out] syncInEdge The register's SyncInEdge field. + /// \param[out] syncInSkipFactor The register's SyncInSkipFactor field. + /// \param[out] syncOutMode The register's SyncOutMode field. + /// \param[out] syncOutPolarity The register's SyncOutPolarity field. + /// \param[out] syncOutSkipFactor The register's SyncOutSkipFactor field. + /// \param[out] syncOutPulseWidth The register's SyncOutPulseWidth field. + void parseSynchronizationControl( + uint8_t * syncInMode, uint8_t * syncInEdge, uint16_t * syncInSkipFactor, uint8_t * syncOutMode, + uint8_t * syncOutPolarity, uint16_t * syncOutSkipFactor, uint32_t * syncOutPulseWidth); + + /// \brief Parses a response from reading the Synchronization Status register. + /// + /// \param[out] syncInCount The register's SyncInCount field. + /// \param[out] syncInTime The register's SyncInTime field. + /// \param[out] syncOutCount The register's SyncOutCount field. + void parseSynchronizationStatus( + uint32_t * syncInCount, uint32_t * syncInTime, uint32_t * syncOutCount); + + /// \brief Parses a response from reading the Filter Basic Control register. + /// + /// \param[out] magMode The register's MagMode field. + /// \param[out] extMagMode The register's ExtMagMode field. + /// \param[out] extAccMode The register's ExtAccMode field. + /// \param[out] extGyroMode The register's ExtGyroMode field. + /// \param[out] gyroLimit The register's GyroLimit field. + void parseFilterBasicControl( + uint8_t * magMode, uint8_t * extMagMode, uint8_t * extAccMode, uint8_t * extGyroMode, + vn::math::vec3f * gyroLimit); + + // Added by jesperh 2021-02-09 + void parseHeaveConfiguration( + float * initialWavePeriod, float * initialWaveAmplitude, float * maxWavePeriod, + float * minWaveAmplitude, float * delayedHeaveCutoffFreq, float * heaveCutoffFreq, + float * heaveRateCutoffFreq); + /// \brief Parses a response from reading the VPE Basic Control register. + /// + /// \param[out] enable The register's Enable field. + /// \param[out] headingMode The register's HeadingMode field. + /// \param[out] filteringMode The register's FilteringMode field. + /// \param[out] tuningMode The register's TuningMode field. + void parseVpeBasicControl( + uint8_t * enable, uint8_t * headingMode, uint8_t * filteringMode, uint8_t * tuningMode); + + /// \brief Parses a response from reading the VPE Magnetometer Basic Tuning register. + /// + /// \param[out] baseTuning The register's BaseTuning field. + /// \param[out] adaptiveTuning The register's AdaptiveTuning field. + /// \param[out] adaptiveFiltering The register's AdaptiveFiltering field. + void parseVpeMagnetometerBasicTuning( + vn::math::vec3f * baseTuning, vn::math::vec3f * adaptiveTuning, + vn::math::vec3f * adaptiveFiltering); + + /// \brief Parses a response from reading the VPE Magnetometer Advanced Tuning register. + /// + /// \param[out] minFiltering The register's MinFiltering field. + /// \param[out] maxFiltering The register's MaxFiltering field. + /// \param[out] maxAdaptRate The register's MaxAdaptRate field. + /// \param[out] disturbanceWindow The register's DisturbanceWindow field. + /// \param[out] maxTuning The register's MaxTuning field. + void parseVpeMagnetometerAdvancedTuning( + vn::math::vec3f * minFiltering, vn::math::vec3f * maxFiltering, float * maxAdaptRate, + float * disturbanceWindow, float * maxTuning); + + /// \brief Parses a response from reading the VPE Accelerometer Basic Tuning register. + /// + /// \param[out] baseTuning The register's BaseTuning field. + /// \param[out] adaptiveTuning The register's AdaptiveTuning field. + /// \param[out] adaptiveFiltering The register's AdaptiveFiltering field. + void parseVpeAccelerometerBasicTuning( + vn::math::vec3f * baseTuning, vn::math::vec3f * adaptiveTuning, + vn::math::vec3f * adaptiveFiltering); + + /// \brief Parses a response from reading the VPE Accelerometer Advanced Tuning register. + /// + /// \param[out] minFiltering The register's MinFiltering field. + /// \param[out] maxFiltering The register's MaxFiltering field. + /// \param[out] maxAdaptRate The register's MaxAdaptRate field. + /// \param[out] disturbanceWindow The register's DisturbanceWindow field. + /// \param[out] maxTuning The register's MaxTuning field. + void parseVpeAccelerometerAdvancedTuning( + vn::math::vec3f * minFiltering, vn::math::vec3f * maxFiltering, float * maxAdaptRate, + float * disturbanceWindow, float * maxTuning); + + /// \brief Parses a response from reading the VPE Gyro Basic Tuning register. + /// + /// \param[out] angularWalkVariance The register's AngularWalkVariance field. + /// \param[out] baseTuning The register's BaseTuning field. + /// \param[out] adaptiveTuning The register's AdaptiveTuning field. + void parseVpeGyroBasicTuning( + vn::math::vec3f * angularWalkVariance, vn::math::vec3f * baseTuning, + vn::math::vec3f * adaptiveTuning); + + /// \brief Parses a response from reading the Filter Startup Gyro Bias register. + /// + /// \param[out] bias The register's Bias field. + void parseFilterStartupGyroBias(vn::math::vec3f * bias); + + /// \brief Parses a response from reading the Magnetometer Calibration Control register. + /// + /// \param[out] hsiMode The register's HSIMode field. + /// \param[out] hsiOutput The register's HSIOutput field. + /// \param[out] convergeRate The register's ConvergeRate field. + void parseMagnetometerCalibrationControl( + uint8_t * hsiMode, uint8_t * hsiOutput, uint8_t * convergeRate); + + /// \brief Parses a response from reading the Calculated Magnetometer Calibration register. + /// + /// \param[out] c The register's C field. + /// \param[out] b The register's B field. + void parseCalculatedMagnetometerCalibration(vn::math::mat3f * c, vn::math::vec3f * b); + + /// \brief Parses a response from reading the Indoor Heading Mode Control register. + /// + /// \param[out] maxRateError The register's Max Rate Error field. + void parseIndoorHeadingModeControl(float * maxRateError); + + /// \brief Parses a response from reading the Velocity Compensation Measurement register. + /// + /// \param[out] velocity The register's Velocity field. + void parseVelocityCompensationMeasurement(vn::math::vec3f * velocity); + + /// \brief Parses a response from reading the Velocity Compensation Control register. + /// + /// \param[out] mode The register's Mode field. + /// \param[out] velocityTuning The register's VelocityTuning field. + /// \param[out] rateTuning The register's RateTuning field. + void parseVelocityCompensationControl(uint8_t * mode, float * velocityTuning, float * rateTuning); + + /// \brief Parses a response from reading the Velocity Compensation Status register. + /// + /// \param[out] x The register's x field. + /// \param[out] xDot The register's xDot field. + /// \param[out] accelOffset The register's accelOffset field. + /// \param[out] omega The register's omega field. + void parseVelocityCompensationStatus( + float * x, float * xDot, vn::math::vec3f * accelOffset, vn::math::vec3f * omega); + + /// \brief Parses a response from reading the IMU Measurements register. + /// + /// \param[out] mag The register's Mag field. + /// \param[out] accel The register's Accel field. + /// \param[out] gyro The register's Gyro field. + /// \param[out] temp The register's Temp field. + /// \param[out] pressure The register's Pressure field. + void parseImuMeasurements( + vn::math::vec3f * mag, vn::math::vec3f * accel, vn::math::vec3f * gyro, float * temp, + float * pressure); + + /// \brief Parses a response from reading the GPS Configuration register (deprecated). + /// + /// \param[out] mode The register's Mode field. + /// \param[out] ppsSource The register's PpsSource field. + void parseGpsConfiguration(uint8_t * mode, uint8_t * ppsSource); + + /// \brief Parses a response from reading the GPS Configuration register. + /// + /// \param[out] mode The register's Mode field. + /// \param[out] ppsSource The register's PpsSource field. + /// \param[out] rate The register's Rate field. + /// \param[out] antPow The register's AntPower field. + void parseGpsConfiguration(uint8_t * mode, uint8_t * ppsSource, uint8_t * rate, uint8_t * antPow); + + /// \brief Parses a response from reading the GPS Antenna Offset register. + /// + /// \param[out] position The register's Position field. + void parseGpsAntennaOffset(vn::math::vec3f * position); + + /// \brief Parses a response from reading the GPS Solution - LLA register. + /// + /// \param[out] time The register's Time field. + /// \param[out] week The register's Week field. + /// \param[out] gpsFix The register's GpsFix field. + /// \param[out] numSats The register's NumSats field. + /// \param[out] lla The register's Lla field. + /// \param[out] nedVel The register's NedVel field. + /// \param[out] nedAcc The register's NedAcc field. + /// \param[out] speedAcc The register's SpeedAcc field. + /// \param[out] timeAcc The register's TimeAcc field. + void parseGpsSolutionLla( + double * time, uint16_t * week, uint8_t * gpsFix, uint8_t * numSats, vn::math::vec3d * lla, + vn::math::vec3f * nedVel, vn::math::vec3f * nedAcc, float * speedAcc, float * timeAcc); + + /// \brief Parses a response from reading the GPS Solution - ECEF register. + /// + /// \param[out] tow The register's Tow field. + /// \param[out] week The register's Week field. + /// \param[out] gpsFix The register's GpsFix field. + /// \param[out] numSats The register's NumSats field. + /// \param[out] position The register's Position field. + /// \param[out] velocity The register's Velocity field. + /// \param[out] posAcc The register's PosAcc field. + /// \param[out] speedAcc The register's SpeedAcc field. + /// \param[out] timeAcc The register's TimeAcc field. + void parseGpsSolutionEcef( + double * tow, uint16_t * week, uint8_t * gpsFix, uint8_t * numSats, vn::math::vec3d * position, + vn::math::vec3f * velocity, vn::math::vec3f * posAcc, float * speedAcc, float * timeAcc); + + /// \brief Parses a response from reading the INS Solution - LLA register. + /// + /// \param[out] time The register's Time field. + /// \param[out] week The register's Week field. + /// \param[out] status The register's Status field. + /// \param[out] yawPitchRoll The register's YawPitchRoll field. + /// \param[out] position The register's Position field. + /// \param[out] nedVel The register's NedVel field. + /// \param[out] attUncertainty The register's AttUncertainty field. + /// \param[out] posUncertainty The register's PosUncertainty field. + /// \param[out] velUncertainty The register's VelUncertainty field. + void parseInsSolutionLla( + double * time, uint16_t * week, uint16_t * status, vn::math::vec3f * yawPitchRoll, + vn::math::vec3d * position, vn::math::vec3f * nedVel, float * attUncertainty, + float * posUncertainty, float * velUncertainty); + + /// \brief Parses a response from reading the INS Solution - ECEF register. + /// + /// \param[out] time The register's Time field. + /// \param[out] week The register's Week field. + /// \param[out] status The register's Status field. + /// \param[out] yawPitchRoll The register's YawPitchRoll field. + /// \param[out] position The register's Position field. + /// \param[out] velocity The register's Velocity field. + /// \param[out] attUncertainty The register's AttUncertainty field. + /// \param[out] posUncertainty The register's PosUncertainty field. + /// \param[out] velUncertainty The register's VelUncertainty field. + void parseInsSolutionEcef( + double * time, uint16_t * week, uint16_t * status, vn::math::vec3f * yawPitchRoll, + vn::math::vec3d * position, vn::math::vec3f * velocity, float * attUncertainty, + float * posUncertainty, float * velUncertainty); + + /// \brief Parses a response from reading the INS Basic Configuration register. + /// + /// \param[out] scenario The register's Scenario field. + /// \param[out] ahrsAiding The register's AhrsAiding field. + void parseInsBasicConfiguration(uint8_t * scenario, uint8_t * ahrsAiding); + + /// \brief Parses a response from reading the INS Basic Configuration register. + /// + /// \param[out] scenario The register's Scenario field. + /// \param[out] ahrsAiding The register's AhrsAiding field. + /// \param[out] estBaseline The register's EstBaseline field. + void parseInsBasicConfiguration(uint8_t * scenario, uint8_t * ahrsAiding, uint8_t * estBaseline); + + /// \brief Parses a response from reading the INS Advanced Configuration register. + /// + /// \param[out] useMag The register's UseMag field. + /// \param[out] usePres The register's UsePres field. + /// \param[out] posAtt The register's PosAtt field. + /// \param[out] velAtt The register's VelAtt field. + /// \param[out] velBias The register's VelBias field. + /// \param[out] useFoam The register's UseFoam field. + /// \param[out] gpsCovType The register's GPSCovType field. + /// \param[out] velCount The register's VelCount field. + /// \param[out] velInit The register's VelInit field. + /// \param[out] moveOrigin The register's MoveOrigin field. + /// \param[out] gpsTimeout The register's GPSTimeout field. + /// \param[out] deltaLimitPos The register's DeltaLimitPos field. + /// \param[out] deltaLimitVel The register's DeltaLimitVel field. + /// \param[out] minPosUncertainty The register's MinPosUncertainty field. + /// \param[out] minVelUncertainty The register's MinVelUncertainty field. + void parseInsAdvancedConfiguration( + uint8_t * useMag, uint8_t * usePres, uint8_t * posAtt, uint8_t * velAtt, uint8_t * velBias, + uint8_t * useFoam, uint8_t * gpsCovType, uint8_t * velCount, float * velInit, + float * moveOrigin, float * gpsTimeout, float * deltaLimitPos, float * deltaLimitVel, + float * minPosUncertainty, float * minVelUncertainty); + + /// \brief Parses a response from reading the INS State - LLA register. + /// + /// \param[out] yawPitchRoll The register's YawPitchRoll field. + /// \param[out] position The register's Position field. + /// \param[out] velocity The register's Velocity field. + /// \param[out] accel The register's Accel field. + /// \param[out] angularRate The register's AngularRate field. + void parseInsStateLla( + vn::math::vec3f * yawPitchRoll, vn::math::vec3d * position, vn::math::vec3f * velocity, + vn::math::vec3f * accel, vn::math::vec3f * angularRate); + + /// \brief Parses a response from reading the INS State - ECEF register. + /// + /// \param[out] yawPitchRoll The register's YawPitchRoll field. + /// \param[out] position The register's Position field. + /// \param[out] velocity The register's Velocity field. + /// \param[out] accel The register's Accel field. + /// \param[out] angularRate The register's AngularRate field. + void parseInsStateEcef( + vn::math::vec3f * yawPitchRoll, vn::math::vec3d * position, vn::math::vec3f * velocity, + vn::math::vec3f * accel, vn::math::vec3f * angularRate); + + /// \brief Parses a response from reading the Startup Filter Bias Estimate register. + /// + /// \param[out] gyroBias The register's GyroBias field. + /// \param[out] accelBias The register's AccelBias field. + /// \param[out] pressureBias The register's PressureBias field. + void parseStartupFilterBiasEstimate( + vn::math::vec3f * gyroBias, vn::math::vec3f * accelBias, float * pressureBias); + + /// \brief Parses a response from reading the Delta Theta and Delta Velocity register. + /// + /// \param[out] deltaTime The register's DeltaTime field. + /// \param[out] deltaTheta The register's DeltaTheta field. + /// \param[out] deltaVelocity The register's DeltaVelocity field. + void parseDeltaThetaAndDeltaVelocity( + float * deltaTime, vn::math::vec3f * deltaTheta, vn::math::vec3f * deltaVelocity); + + /// \brief Parses a response from reading the Delta Theta and Delta Velocity Configuration register (deprecated). + /// + /// \param[out] integrationFrame The register's IntegrationFrame field. + /// \param[out] gyroCompensation The register's GyroCompensation field. + /// \param[out] accelCompensation The register's AccelCompensation field. + void parseDeltaThetaAndDeltaVelocityConfiguration( + uint8_t * integrationFrame, uint8_t * gyroCompensation, uint8_t * accelCompensation); + + /// \brief Parses a response from reading the Delta Theta and Delta Velocity Configuration register. + /// + /// \param[out] integrationFrame The register's IntegrationFrame field. + /// \param[out] gyroCompensation The register's GyroCompensation field. + /// \param[out] accelCompensation The register's AccelCompensation field. + /// \param[out] earthRateCorrection The register's EarthRateCorrection field. + void parseDeltaThetaAndDeltaVelocityConfiguration( + uint8_t * integrationFrame, uint8_t * gyroCompensation, uint8_t * accelCompensation, + uint8_t * earthRateCorrection); + + /// \brief Parses a response from reading the Reference Vector Configuration register. + /// + /// \param[out] useMagModel The register's UseMagModel field. + /// \param[out] useGravityModel The register's UseGravityModel field. + /// \param[out] recalcThreshold The register's RecalcThreshold field. + /// \param[out] year The register's Year field. + /// \param[out] position The register's Position field. + void parseReferenceVectorConfiguration( + uint8_t * useMagModel, uint8_t * useGravityModel, uint32_t * recalcThreshold, float * year, + vn::math::vec3d * position); + + /// \brief Parses a response from reading the Gyro Compensation register. + /// + /// \param[out] c The register's C field. + /// \param[out] b The register's B field. + void parseGyroCompensation(vn::math::mat3f * c, vn::math::vec3f * b); + + /// \brief Parses a response from reading the IMU Filtering Configuration register. + /// + /// \param[out] magWindowSize The register's MagWindowSize field. + /// \param[out] accelWindowSize The register's AccelWindowSize field. + /// \param[out] gyroWindowSize The register's GyroWindowSize field. + /// \param[out] tempWindowSize The register's TempWindowSize field. + /// \param[out] presWindowSize The register's PresWindowSize field. + /// \param[out] magFilterMode The register's MagFilterMode field. + /// \param[out] accelFilterMode The register's AccelFilterMode field. + /// \param[out] gyroFilterMode The register's GyroFilterMode field. + /// \param[out] tempFilterMode The register's TempFilterMode field. + /// \param[out] presFilterMode The register's PresFilterMode field. + void parseImuFilteringConfiguration( + 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 Parses a response from reading the GPS Compass Baseline register. + /// + /// \param[out] position The register's Position field. + /// \param[out] uncertainty The register's Uncertainty field. + void parseGpsCompassBaseline(vn::math::vec3f * position, vn::math::vec3f * uncertainty); + + /// \brief Parses a response from reading the GPS Compass Estimated Baseline register. + /// + /// \param[out] estBaselineUsed The register's EstBaselineUsed field. + /// \param[out] numMeas The register's NumMeas field. + /// \param[out] position The register's Position field. + /// \param[out] uncertainty The register's Uncertainty field. + void parseGpsCompassEstimatedBaseline( + uint8_t * estBaselineUsed, uint16_t * numMeas, vn::math::vec3f * position, + vn::math::vec3f * uncertainty); + + /// \brief Parses a response from reading the IMU Rate Configuration register. + /// + /// \param[out] imuRate The register's imuRate field. + /// \param[out] navDivisor The register's NavDivisor field. + /// \param[out] filterTargetRate The register's filterTargetRate field. + /// \param[out] filterMinRate The register's filterMinRate field. + void parseImuRateConfiguration( + uint16_t * imuRate, uint16_t * navDivisor, float * filterTargetRate, float * filterMinRate); + + /// \brief Parses a response from reading the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register. + /// + /// \param[out] yawPitchRoll The register's YawPitchRoll field. + /// \param[out] bodyAccel The register's BodyAccel field. + /// \param[out] gyro The register's Gyro field. + void parseYawPitchRollTrueBodyAccelerationAndAngularRates( + vn::math::vec3f * yawPitchRoll, vn::math::vec3f * bodyAccel, vn::math::vec3f * gyro); + + /// \brief Parses a response from reading the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register. + /// + /// \param[out] yawPitchRoll The register's YawPitchRoll field. + /// \param[out] inertialAccel The register's InertialAccel field. + /// \param[out] gyro The register's Gyro field. + void parseYawPitchRollTrueInertialAccelerationAndAngularRates( + vn::math::vec3f * yawPitchRoll, vn::math::vec3f * inertialAccel, vn::math::vec3f * gyro); + + /// \} private: + void ensureCanExtract(size_t numOfBytes); - void ensureCanExtract(size_t numOfBytes); - - bool _isPacketDataMine; - size_t _length; - char *_data; - size_t _curExtractLoc; + bool _isPacketDataMine; + size_t _length; + char * _data; + size_t _curExtractLoc; }; -} -} -} +} // namespace uart +} // namespace protocol +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/packetfinder.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/packetfinder.h index 5611ed69..9b64a39f 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/packetfinder.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/packetfinder.h @@ -2,16 +2,19 @@ #define _VNPROTOCOL_UART_PACKETFINDER_H_ #include "nocopy.h" -#include "vntime.h" #include "packet.h" +#include "vntime.h" #if PYTHON - #include "vn/util/boostpython.h" +#include "vn/util/boostpython.h" #endif -namespace vn { -namespace protocol { -namespace uart { +namespace vn +{ +namespace protocol +{ +namespace uart +{ /// \brief Helps with management of communication with a sensor using the UART /// protocol. @@ -24,80 +27,81 @@ namespace uart { /// incremented for each byte received. class vn_proglib_DLLEXPORT PacketFinder : private util::NoCopy { - public: + /// \brief Defines the signature for a method that can receive + /// notifications of new valid packets found. + /// + /// \param[in] userData Pointer to user data that was initially supplied + /// when the callback was registered via registerPossiblePacketFoundHandler. + /// \param[in] possiblePacket The possible packet that was found. + /// \param[in] packetStartRunningIndex The running index of the start of + /// the packet. + /// \param[in] timestamp The timestamp the packet was found. + typedef void (*ValidPacketFoundHandler)( + void * userData, Packet & packet, size_t runningIndexOfPacketStart, xplat::TimeStamp timestamp); + + /// \brief Creates a new /ref PacketFinder with internal buffers to store + /// incoming bytes and alert when valid packets are received. + PacketFinder(); + + /// \brief Creates a new /ref PacketFinder with an internal buffer the size + /// specified. + /// + /// \param[in] internalReceiveBufferSize The number of bytes to make the + /// internal buffer. + explicit PacketFinder(size_t internalReceiveBufferSize); + + ~PacketFinder(); + + /// \brief Adds new data to the internal buffers and processes the received + /// data to determine if any new received packets are available. + /// + /// \param[in] data The data buffer containing the received data. + /// \param[in] length The number of bytes of data in the buffer. + void processReceivedData(char data[], size_t length); + void processReceivedData(char data[], size_t length, bool bootloaderFilter); + + /// \brief Adds new data to the internal buffers and processes the received + /// data to determine if any new received packets are available. + /// + /// \param[in] data The data buffer containing the received data. + /// \param[in] length The number of bytes of data in the buffer. + /// \param[in] timestamp The time when the data was received. + void processReceivedData( + char data[], size_t length, bool bootloaderFilter, xplat::TimeStamp timestamp); + +#if PYTHON + + void processReceivedData(boost::python::list data); - /// \brief Defines the signature for a method that can receive - /// notifications of new valid packets found. - /// - /// \param[in] userData Pointer to user data that was initially supplied - /// when the callback was registered via registerPossiblePacketFoundHandler. - /// \param[in] possiblePacket The possible packet that was found. - /// \param[in] packetStartRunningIndex The running index of the start of - /// the packet. - /// \param[in] timestamp The timestamp the packet was found. - typedef void (*ValidPacketFoundHandler)(void* userData, Packet& packet, size_t runningIndexOfPacketStart, xplat::TimeStamp timestamp); - - /// \brief Creates a new /ref PacketFinder with internal buffers to store - /// incoming bytes and alert when valid packets are received. - PacketFinder(); - - /// \brief Creates a new /ref PacketFinder with an internal buffer the size - /// specified. - /// - /// \param[in] internalReceiveBufferSize The number of bytes to make the - /// internal buffer. - explicit PacketFinder(size_t internalReceiveBufferSize); - - ~PacketFinder(); - - /// \brief Adds new data to the internal buffers and processes the received - /// data to determine if any new received packets are available. - /// - /// \param[in] data The data buffer containing the received data. - /// \param[in] length The number of bytes of data in the buffer. - void processReceivedData(char data[], size_t length); - void processReceivedData(char data[], size_t length, bool bootloaderFilter); - - /// \brief Adds new data to the internal buffers and processes the received - /// data to determine if any new received packets are available. - /// - /// \param[in] data The data buffer containing the received data. - /// \param[in] length The number of bytes of data in the buffer. - /// \param[in] timestamp The time when the data was received. - void processReceivedData(char data[], size_t length, bool bootloaderFilter, xplat::TimeStamp timestamp); - - #if PYTHON - - void processReceivedData(boost::python::list data); - - #endif - - /// \brief Registers a callback method for notification when a new possible - /// packet is found. - /// - /// \param[in] userData Pointer to user data, which will be provided to the - /// callback method. - /// \param[in] handler The callback method. - void registerPossiblePacketFoundHandler(void* userData, ValidPacketFoundHandler handler); - - /// \brief Unregisters the registered callback method. - void unregisterPossiblePacketFoundHandler(); - - #if PYTHON - - boost::python::object* register_packet_found_handler(/*boost::python::object* callable*/ PyObject* callable); - //void register_packet_found_handler(boost::python::object* callable); - - #endif +#endif + + /// \brief Registers a callback method for notification when a new possible + /// packet is found. + /// + /// \param[in] userData Pointer to user data, which will be provided to the + /// callback method. + /// \param[in] handler The callback method. + void registerPossiblePacketFoundHandler(void * userData, ValidPacketFoundHandler handler); + + /// \brief Unregisters the registered callback method. + void unregisterPossiblePacketFoundHandler(); + +#if PYTHON + + boost::python::object * register_packet_found_handler( + /*boost::python::object* callable*/ PyObject * callable); + //void register_packet_found_handler(boost::python::object* callable); + +#endif private: - struct Impl; - Impl *_pi; + struct Impl; + Impl * _pi; }; -} -} -} +} // namespace uart +} // namespace protocol +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/port.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/port.h index 3533f077..7f400de7 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/port.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/port.h @@ -5,78 +5,77 @@ #include "export.h" -namespace vn { -namespace xplat { +namespace vn +{ +namespace xplat +{ /// \brief Interface for a simple port. class vn_proglib_DLLEXPORT IPort { + // Types ////////////////////////////////////////////////////////////////// - // Types ////////////////////////////////////////////////////////////////// - public: + /// \brief Callback handler signature that can receive notification when + /// new data has been received on the port. + /// + /// \param[in] userData Pointer to user data that was initially supplied + /// when the callback was registered with registerDataReceivedHandler. + typedef void (*DataReceivedHandler)(void * userData); - /// \brief Callback handler signature that can receive notification when - /// new data has been received on the port. - /// - /// \param[in] userData Pointer to user data that was initially supplied - /// when the callback was registered with registerDataReceivedHandler. - typedef void(*DataReceivedHandler)(void* userData); - - // Public Methods ///////////////////////////////////////////////////////// + // Public Methods ///////////////////////////////////////////////////////// public: + virtual ~IPort() = 0; + + /// \brief Opens the simple port. + virtual void open() = 0; + + /// \brief Closes the simple port. + virtual void close() = 0; + + /// \brief Indicates if the simple port is open. + /// + /// \return true if the serial port is open; otherwise false. + virtual bool isOpen() = 0; + + /// \brief Writes out data to the simple port. + /// + /// \param[in] data The data array to write out. + /// \param[in] length The length of the data array to write out. + virtual void write(const char data[], size_t length) = 0; + + /// \brief Allows reading data from the simple port. + /// + /// \param[out] dataBuffer The data buffer to write the read data bytes to. + /// \param[in] numOfBytesToRead The number of bytes to attempt reading from + /// the simple port. + /// \param[out] numOfBytesActuallyRead The number of bytes actually read + /// from the simple port. + virtual void read( + char dataBuffer[], size_t numOfBytesToRead, size_t & numOfBytesActuallyRead) = 0; + + /// \brief Registers a callback method for notification when new data is + /// received on the port. + /// + /// \param[in] userData Pointer to user data, which will be provided to the + /// callback method. + /// \param[in] handler The callback method. + virtual void registerDataReceivedHandler(void * userData, DataReceivedHandler handler) = 0; + + /// \brief Unregisters the registered callback method. + virtual void unregisterDataReceivedHandler() = 0; + +#if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 + + virtual void stopThread() {} + virtual bool threadStopped() { return false; } + virtual void resumeThread() {} - virtual ~IPort() = 0; - - /// \brief Opens the simple port. - virtual void open() = 0; - - /// \brief Closes the simple port. - virtual void close() = 0; - - /// \brief Indicates if the simple port is open. - /// - /// \return true if the serial port is open; otherwise false. - virtual bool isOpen() = 0; - - /// \brief Writes out data to the simple port. - /// - /// \param[in] data The data array to write out. - /// \param[in] length The length of the data array to write out. - virtual void write(const char data[], size_t length) = 0; - - /// \brief Allows reading data from the simple port. - /// - /// \param[out] dataBuffer The data buffer to write the read data bytes to. - /// \param[in] numOfBytesToRead The number of bytes to attempt reading from - /// the simple port. - /// \param[out] numOfBytesActuallyRead The number of bytes actually read - /// from the simple port. - virtual void read(char dataBuffer[], size_t numOfBytesToRead, size_t &numOfBytesActuallyRead) = 0; - - /// \brief Registers a callback method for notification when new data is - /// received on the port. - /// - /// \param[in] userData Pointer to user data, which will be provided to the - /// callback method. - /// \param[in] handler The callback method. - virtual void registerDataReceivedHandler(void* userData, DataReceivedHandler handler) = 0; - - /// \brief Unregisters the registered callback method. - virtual void unregisterDataReceivedHandler() = 0; - - #if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 - - virtual void stopThread(){} - virtual bool threadStopped(){ return false; } - virtual void resumeThread(){} - - #endif - +#endif }; -} -} +} // namespace xplat +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/position.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/position.h index dcad653d..e01cc1e4 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/position.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/position.h @@ -3,48 +3,43 @@ #include "vector.h" -namespace vn { -namespace math { +namespace vn +{ +namespace math +{ /// \brief Representation of a position/location. class PositionD { private: - - enum PositionType - { - POS_LLA, - POS_ECEF - }; + enum PositionType { POS_LLA, POS_ECEF }; public: - // TEMP - PositionD() { } + // TEMP + PositionD() {} private: - - PositionD(PositionType type, vec3d pos); + PositionD(PositionType type, vec3d pos); public: - - /// \brief Creates a new PositionD from a latitude, longitude, altitude. - /// - /// \param[in] lla The position expressed as a latitude, longitude, altitude. - /// \return The new PositionD. - static PositionD fromLla(vec3d lla); - - /// \brief Creates a new PositionD from an earth-centered, earth-fixed. - /// - /// \param[in] ecef The position expressed as an earth-centered, earth-fixed. - /// \return The new PositionD. - static PositionD fromEcef(vec3d ecef); + /// \brief Creates a new PositionD from a latitude, longitude, altitude. + /// + /// \param[in] lla The position expressed as a latitude, longitude, altitude. + /// \return The new PositionD. + static PositionD fromLla(vec3d lla); + + /// \brief Creates a new PositionD from an earth-centered, earth-fixed. + /// + /// \param[in] ecef The position expressed as an earth-centered, earth-fixed. + /// \return The new PositionD. + static PositionD fromEcef(vec3d ecef); private: - PositionType _underlyingType; - vec3d _data; + PositionType _underlyingType; + vec3d _data; }; -} -} +} // namespace math +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/registers.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/registers.h index a49c3d8e..80ee8a9a 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/registers.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/registers.h @@ -2,12 +2,14 @@ #define _VNSENSORS_REGISTERS_H_ #include "int.h" -#include "vector.h" #include "matrix.h" #include "types.h" +#include "vector.h" -namespace vn { -namespace sensors { +namespace vn +{ +namespace sensors +{ /// \defgroup registerStructures Register Structures /// \brief These structures represent the various registers on a VecotorNav @@ -23,1530 +25,1350 @@ namespace sensors { /// structure. struct BinaryOutputRegister { - protocol::uart::AsyncMode asyncMode; ///< The asyncMode field. - uint16_t rateDivisor; ///< The rateDivisor field. - protocol::uart::CommonGroup commonField; ///< Group 1 (Common) - protocol::uart::TimeGroup timeField; ///< Group 2 (Time) - protocol::uart::ImuGroup imuField; ///< Group 3 (IMU) - protocol::uart::GpsGroup gpsField; ///< Group 4 (GPS) - protocol::uart::AttitudeGroup attitudeField; ///< Group 5 (Attitude) - protocol::uart::InsGroup insField; ///< Group 6 (INS) - protocol::uart::GpsGroup gps2Field; ///< Group 7 (GPS2) - - BinaryOutputRegister() : - asyncMode(protocol::uart::ASYNCMODE_NONE), - rateDivisor(0), - commonField(protocol::uart::COMMONGROUP_NONE), - timeField(protocol::uart::TIMEGROUP_NONE), - imuField(protocol::uart::IMUGROUP_NONE), - gpsField(protocol::uart::GPSGROUP_NONE), - attitudeField(protocol::uart::ATTITUDEGROUP_NONE), - insField(protocol::uart::INSGROUP_NONE), + protocol::uart::AsyncMode asyncMode; ///< The asyncMode field. + uint16_t rateDivisor; ///< The rateDivisor field. + protocol::uart::CommonGroup commonField; ///< Group 1 (Common) + protocol::uart::TimeGroup timeField; ///< Group 2 (Time) + protocol::uart::ImuGroup imuField; ///< Group 3 (IMU) + protocol::uart::GpsGroup gpsField; ///< Group 4 (GPS) + protocol::uart::AttitudeGroup attitudeField; ///< Group 5 (Attitude) + protocol::uart::InsGroup insField; ///< Group 6 (INS) + protocol::uart::GpsGroup gps2Field; ///< Group 7 (GPS2) + + BinaryOutputRegister() + : asyncMode(protocol::uart::ASYNCMODE_NONE), + rateDivisor(0), + commonField(protocol::uart::COMMONGROUP_NONE), + timeField(protocol::uart::TIMEGROUP_NONE), + imuField(protocol::uart::IMUGROUP_NONE), + gpsField(protocol::uart::GPSGROUP_NONE), + attitudeField(protocol::uart::ATTITUDEGROUP_NONE), + insField(protocol::uart::INSGROUP_NONE), gps2Field(protocol::uart::GPSGROUP_NONE) - { } - - /// \brief Creates an initializes a new BinaryOutputRegister structure. - /// - /// \param[in] asyncModeIn Value to initialize the asyncMode field with. - /// \param[in] rateDivisorIn Value to initialize the rateDivisor field with. - /// \param[in] commonFieldIn Value to initialize field 1 (Common) with. - /// \param[in] timeFieldIn Value to initialize field 2 (Time) with. - /// \param[in] imuFieldIn Value to initialize field 3 (IMU) with. - /// \param[in] gpsFieldIn Value to initialize field 4 (GPS) with. - /// \param[in] attitudeFieldIn Value to initialize field 5 (Attitude) with. - /// \param[in] insFieldIn Value to initialize field 6 (INS) with. + { + } + + /// \brief Creates an initializes a new BinaryOutputRegister structure. + /// + /// \param[in] asyncModeIn Value to initialize the asyncMode field with. + /// \param[in] rateDivisorIn Value to initialize the rateDivisor field with. + /// \param[in] commonFieldIn Value to initialize field 1 (Common) with. + /// \param[in] timeFieldIn Value to initialize field 2 (Time) with. + /// \param[in] imuFieldIn Value to initialize field 3 (IMU) with. + /// \param[in] gpsFieldIn Value to initialize field 4 (GPS) with. + /// \param[in] attitudeFieldIn Value to initialize field 5 (Attitude) with. + /// \param[in] insFieldIn Value to initialize field 6 (INS) with. /// \param[in] gps2FieldIn Value to initialize field 7 (GPS2) with. - BinaryOutputRegister( - protocol::uart::AsyncMode asyncModeIn, - uint16_t rateDivisorIn, - protocol::uart::CommonGroup commonFieldIn, - protocol::uart::TimeGroup timeFieldIn, - protocol::uart::ImuGroup imuFieldIn, - protocol::uart::GpsGroup gpsFieldIn, - protocol::uart::AttitudeGroup attitudeFieldIn, - protocol::uart::InsGroup insFieldIn, - protocol::uart::GpsGroup gps2FieldIn) : - asyncMode(asyncModeIn), - rateDivisor(rateDivisorIn), - commonField(commonFieldIn), - timeField(timeFieldIn), - imuField(imuFieldIn), - gpsField(gpsFieldIn), - attitudeField(attitudeFieldIn), - insField(insFieldIn), + BinaryOutputRegister( + protocol::uart::AsyncMode asyncModeIn, uint16_t rateDivisorIn, + protocol::uart::CommonGroup commonFieldIn, protocol::uart::TimeGroup timeFieldIn, + protocol::uart::ImuGroup imuFieldIn, protocol::uart::GpsGroup gpsFieldIn, + protocol::uart::AttitudeGroup attitudeFieldIn, protocol::uart::InsGroup insFieldIn, + protocol::uart::GpsGroup gps2FieldIn) + : asyncMode(asyncModeIn), + rateDivisor(rateDivisorIn), + commonField(commonFieldIn), + timeField(timeFieldIn), + imuField(imuFieldIn), + gpsField(gpsFieldIn), + attitudeField(attitudeFieldIn), + insField(insFieldIn), gps2Field(gps2FieldIn) - { } - - BinaryOutputRegister( - uint16_t asyncModeIn, - uint16_t rateDivisorIn, - uint16_t commonFieldIn, - uint16_t timeFieldIn, - uint16_t imuFieldIn, - uint16_t gpsFieldIn, - uint16_t attitudeFieldIn, - uint16_t insFieldIn, - uint16_t gps2FieldIn) : - asyncMode(static_cast(asyncModeIn)), - rateDivisor(rateDivisorIn), - commonField(static_cast(commonFieldIn)), - timeField(static_cast(timeFieldIn)), - imuField(static_cast(imuFieldIn)), - gpsField(static_cast(gpsFieldIn)), - attitudeField(static_cast(attitudeFieldIn)), - insField(static_cast(insFieldIn)), + { + } + + BinaryOutputRegister( + uint16_t asyncModeIn, uint16_t rateDivisorIn, uint16_t commonFieldIn, uint16_t timeFieldIn, + uint16_t imuFieldIn, uint16_t gpsFieldIn, uint16_t attitudeFieldIn, uint16_t insFieldIn, + uint16_t gps2FieldIn) + : asyncMode(static_cast(asyncModeIn)), + rateDivisor(rateDivisorIn), + commonField(static_cast(commonFieldIn)), + timeField(static_cast(timeFieldIn)), + imuField(static_cast(imuFieldIn)), + gpsField(static_cast(gpsFieldIn)), + attitudeField(static_cast(attitudeFieldIn)), + insField(static_cast(insFieldIn)), gps2Field(static_cast(gps2FieldIn)) - { } + { + } }; /// \brief Structure representing the Quaternion, Magnetic, Acceleration and Angular Rates register. struct QuaternionMagneticAccelerationAndAngularRatesRegister { - vn::math::vec4f quat; ///< The quat field. - vn::math::vec3f mag; ///< The mag field. - vn::math::vec3f accel; ///< The accel field. - vn::math::vec3f gyro; ///< The gyro field. - - QuaternionMagneticAccelerationAndAngularRatesRegister() { } - - /// \brief Creates an initializes a new QuaternionMagneticAccelerationAndAngularRatesRegister structure. - /// - /// \param[in] quatIn Value to initialize the quat field with. - /// \param[in] magIn Value to initialize the mag field with. - /// \param[in] accelIn Value to initialize the accel field with. - /// \param[in] gyroIn Value to initialize the gyro field with. - QuaternionMagneticAccelerationAndAngularRatesRegister( - vn::math::vec4f quatIn, - vn::math::vec3f magIn, - vn::math::vec3f accelIn, - vn::math::vec3f gyroIn) : - quat(quatIn), - mag(magIn), - accel(accelIn), - gyro(gyroIn) - { } - + vn::math::vec4f quat; ///< The quat field. + vn::math::vec3f mag; ///< The mag field. + vn::math::vec3f accel; ///< The accel field. + vn::math::vec3f gyro; ///< The gyro field. + + QuaternionMagneticAccelerationAndAngularRatesRegister() {} + + /// \brief Creates an initializes a new QuaternionMagneticAccelerationAndAngularRatesRegister structure. + /// + /// \param[in] quatIn Value to initialize the quat field with. + /// \param[in] magIn Value to initialize the mag field with. + /// \param[in] accelIn Value to initialize the accel field with. + /// \param[in] gyroIn Value to initialize the gyro field with. + QuaternionMagneticAccelerationAndAngularRatesRegister( + vn::math::vec4f quatIn, vn::math::vec3f magIn, vn::math::vec3f accelIn, vn::math::vec3f gyroIn) + : quat(quatIn), mag(magIn), accel(accelIn), gyro(gyroIn) + { + } }; /// \brief Structure representing the Magnetic, Acceleration and Angular Rates register. struct MagneticAccelerationAndAngularRatesRegister { - vn::math::vec3f mag; ///< The mag field. - vn::math::vec3f accel; ///< The accel field. - vn::math::vec3f gyro; ///< The gyro field. - - MagneticAccelerationAndAngularRatesRegister() { } - - /// \brief Creates an initializes a new MagneticAccelerationAndAngularRatesRegister structure. - /// - /// \param[in] magIn Value to initialize the mag field with. - /// \param[in] accelIn Value to initialize the accel field with. - /// \param[in] gyroIn Value to initialize the gyro field with. - MagneticAccelerationAndAngularRatesRegister( - vn::math::vec3f magIn, - vn::math::vec3f accelIn, - vn::math::vec3f gyroIn) : - mag(magIn), - accel(accelIn), - gyro(gyroIn) - { } - + vn::math::vec3f mag; ///< The mag field. + vn::math::vec3f accel; ///< The accel field. + vn::math::vec3f gyro; ///< The gyro field. + + MagneticAccelerationAndAngularRatesRegister() {} + + /// \brief Creates an initializes a new MagneticAccelerationAndAngularRatesRegister structure. + /// + /// \param[in] magIn Value to initialize the mag field with. + /// \param[in] accelIn Value to initialize the accel field with. + /// \param[in] gyroIn Value to initialize the gyro field with. + MagneticAccelerationAndAngularRatesRegister( + vn::math::vec3f magIn, vn::math::vec3f accelIn, vn::math::vec3f gyroIn) + : mag(magIn), accel(accelIn), gyro(gyroIn) + { + } }; /// \brief Structure representing the Magnetic and Gravity Reference Vectors register. struct MagneticAndGravityReferenceVectorsRegister { - vn::math::vec3f magRef; ///< The magRef field. - vn::math::vec3f accRef; ///< The accRef field. - - MagneticAndGravityReferenceVectorsRegister() { } - - /// \brief Creates an initializes a new MagneticAndGravityReferenceVectorsRegister structure. - /// - /// \param[in] magRefIn Value to initialize the magRef field with. - /// \param[in] accRefIn Value to initialize the accRef field with. - MagneticAndGravityReferenceVectorsRegister( - vn::math::vec3f magRefIn, - vn::math::vec3f accRefIn) : - magRef(magRefIn), - accRef(accRefIn) - { } - + vn::math::vec3f magRef; ///< The magRef field. + vn::math::vec3f accRef; ///< The accRef field. + + MagneticAndGravityReferenceVectorsRegister() {} + + /// \brief Creates an initializes a new MagneticAndGravityReferenceVectorsRegister structure. + /// + /// \param[in] magRefIn Value to initialize the magRef field with. + /// \param[in] accRefIn Value to initialize the accRef field with. + MagneticAndGravityReferenceVectorsRegister(vn::math::vec3f magRefIn, vn::math::vec3f accRefIn) + : magRef(magRefIn), accRef(accRefIn) + { + } }; /// \brief Structure representing the Filter Measurements Variance Parameters register. struct FilterMeasurementsVarianceParametersRegister { - float angularWalkVariance; ///< The angularWalkVariance field. - vn::math::vec3f angularRateVariance; ///< The angularRateVariance field. - vn::math::vec3f magneticVariance; ///< The magneticVariance field. - vn::math::vec3f accelerationVariance; ///< The accelerationVariance field. - - FilterMeasurementsVarianceParametersRegister() { } - - /// \brief Creates an initializes a new FilterMeasurementsVarianceParametersRegister structure. - /// - /// \param[in] angularWalkVarianceIn Value to initialize the angularWalkVariance field with. - /// \param[in] angularRateVarianceIn Value to initialize the angularRateVariance field with. - /// \param[in] magneticVarianceIn Value to initialize the magneticVariance field with. - /// \param[in] accelerationVarianceIn Value to initialize the accelerationVariance field with. - FilterMeasurementsVarianceParametersRegister( - float angularWalkVarianceIn, - vn::math::vec3f angularRateVarianceIn, - vn::math::vec3f magneticVarianceIn, - vn::math::vec3f accelerationVarianceIn) : - angularWalkVariance(angularWalkVarianceIn), - angularRateVariance(angularRateVarianceIn), - magneticVariance(magneticVarianceIn), - accelerationVariance(accelerationVarianceIn) - { } - + float angularWalkVariance; ///< The angularWalkVariance field. + vn::math::vec3f angularRateVariance; ///< The angularRateVariance field. + vn::math::vec3f magneticVariance; ///< The magneticVariance field. + vn::math::vec3f accelerationVariance; ///< The accelerationVariance field. + + FilterMeasurementsVarianceParametersRegister() {} + + /// \brief Creates an initializes a new FilterMeasurementsVarianceParametersRegister structure. + /// + /// \param[in] angularWalkVarianceIn Value to initialize the angularWalkVariance field with. + /// \param[in] angularRateVarianceIn Value to initialize the angularRateVariance field with. + /// \param[in] magneticVarianceIn Value to initialize the magneticVariance field with. + /// \param[in] accelerationVarianceIn Value to initialize the accelerationVariance field with. + FilterMeasurementsVarianceParametersRegister( + float angularWalkVarianceIn, vn::math::vec3f angularRateVarianceIn, + vn::math::vec3f magneticVarianceIn, vn::math::vec3f accelerationVarianceIn) + : angularWalkVariance(angularWalkVarianceIn), + angularRateVariance(angularRateVarianceIn), + magneticVariance(magneticVarianceIn), + accelerationVariance(accelerationVarianceIn) + { + } }; /// \brief Structure representing the Magnetometer Compensation register. struct MagnetometerCompensationRegister { - vn::math::mat3f c; ///< The c field. - vn::math::vec3f b; ///< The b field. - - MagnetometerCompensationRegister() { } - - /// \brief Creates an initializes a new MagnetometerCompensationRegister structure. - /// - /// \param[in] cIn Value to initialize the c field with. - /// \param[in] bIn Value to initialize the b field with. - MagnetometerCompensationRegister( - vn::math::mat3f cIn, - vn::math::vec3f bIn) : - c(cIn), - b(bIn) - { } + vn::math::mat3f c; ///< The c field. + vn::math::vec3f b; ///< The b field. + + MagnetometerCompensationRegister() {} + /// \brief Creates an initializes a new MagnetometerCompensationRegister structure. + /// + /// \param[in] cIn Value to initialize the c field with. + /// \param[in] bIn Value to initialize the b field with. + MagnetometerCompensationRegister(vn::math::mat3f cIn, vn::math::vec3f bIn) : c(cIn), b(bIn) {} }; /// \brief Structure representing the Filter Active Tuning Parameters register. struct FilterActiveTuningParametersRegister { - float magneticDisturbanceGain; ///< The magneticDisturbanceGain field. - float accelerationDisturbanceGain; ///< The accelerationDisturbanceGain field. - float magneticDisturbanceMemory; ///< The magneticDisturbanceMemory field. - float accelerationDisturbanceMemory; ///< The accelerationDisturbanceMemory field. - - FilterActiveTuningParametersRegister() { } - - /// \brief Creates an initializes a new FilterActiveTuningParametersRegister structure. - /// - /// \param[in] magneticDisturbanceGainIn Value to initialize the magneticDisturbanceGain field with. - /// \param[in] accelerationDisturbanceGainIn Value to initialize the accelerationDisturbanceGain field with. - /// \param[in] magneticDisturbanceMemoryIn Value to initialize the magneticDisturbanceMemory field with. - /// \param[in] accelerationDisturbanceMemoryIn Value to initialize the accelerationDisturbanceMemory field with. - FilterActiveTuningParametersRegister( - float magneticDisturbanceGainIn, - float accelerationDisturbanceGainIn, - float magneticDisturbanceMemoryIn, - float accelerationDisturbanceMemoryIn) : - magneticDisturbanceGain(magneticDisturbanceGainIn), - accelerationDisturbanceGain(accelerationDisturbanceGainIn), - magneticDisturbanceMemory(magneticDisturbanceMemoryIn), - accelerationDisturbanceMemory(accelerationDisturbanceMemoryIn) - { } - + float magneticDisturbanceGain; ///< The magneticDisturbanceGain field. + float accelerationDisturbanceGain; ///< The accelerationDisturbanceGain field. + float magneticDisturbanceMemory; ///< The magneticDisturbanceMemory field. + float accelerationDisturbanceMemory; ///< The accelerationDisturbanceMemory field. + + FilterActiveTuningParametersRegister() {} + + /// \brief Creates an initializes a new FilterActiveTuningParametersRegister structure. + /// + /// \param[in] magneticDisturbanceGainIn Value to initialize the magneticDisturbanceGain field with. + /// \param[in] accelerationDisturbanceGainIn Value to initialize the accelerationDisturbanceGain field with. + /// \param[in] magneticDisturbanceMemoryIn Value to initialize the magneticDisturbanceMemory field with. + /// \param[in] accelerationDisturbanceMemoryIn Value to initialize the accelerationDisturbanceMemory field with. + FilterActiveTuningParametersRegister( + float magneticDisturbanceGainIn, float accelerationDisturbanceGainIn, + float magneticDisturbanceMemoryIn, float accelerationDisturbanceMemoryIn) + : magneticDisturbanceGain(magneticDisturbanceGainIn), + accelerationDisturbanceGain(accelerationDisturbanceGainIn), + magneticDisturbanceMemory(magneticDisturbanceMemoryIn), + accelerationDisturbanceMemory(accelerationDisturbanceMemoryIn) + { + } }; /// \brief Structure representing the Acceleration Compensation register. struct AccelerationCompensationRegister { - vn::math::mat3f c; ///< The c field. - vn::math::vec3f b; ///< The b field. - - AccelerationCompensationRegister() { } - - /// \brief Creates an initializes a new AccelerationCompensationRegister structure. - /// - /// \param[in] cIn Value to initialize the c field with. - /// \param[in] bIn Value to initialize the b field with. - AccelerationCompensationRegister( - vn::math::mat3f cIn, - vn::math::vec3f bIn) : - c(cIn), - b(bIn) - { } + vn::math::mat3f c; ///< The c field. + vn::math::vec3f b; ///< The b field. + + AccelerationCompensationRegister() {} + /// \brief Creates an initializes a new AccelerationCompensationRegister structure. + /// + /// \param[in] cIn Value to initialize the c field with. + /// \param[in] bIn Value to initialize the b field with. + AccelerationCompensationRegister(vn::math::mat3f cIn, vn::math::vec3f bIn) : c(cIn), b(bIn) {} }; /// \brief Structure representing the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register. struct YawPitchRollMagneticAccelerationAndAngularRatesRegister { - vn::math::vec3f yawPitchRoll; ///< The yawPitchRoll field. - vn::math::vec3f mag; ///< The mag field. - vn::math::vec3f accel; ///< The accel field. - vn::math::vec3f gyro; ///< The gyro field. - - YawPitchRollMagneticAccelerationAndAngularRatesRegister() { } - - /// \brief Creates an initializes a new YawPitchRollMagneticAccelerationAndAngularRatesRegister structure. - /// - /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. - /// \param[in] magIn Value to initialize the mag field with. - /// \param[in] accelIn Value to initialize the accel field with. - /// \param[in] gyroIn Value to initialize the gyro field with. - YawPitchRollMagneticAccelerationAndAngularRatesRegister( - vn::math::vec3f yawPitchRollIn, - vn::math::vec3f magIn, - vn::math::vec3f accelIn, - vn::math::vec3f gyroIn) : - yawPitchRoll(yawPitchRollIn), - mag(magIn), - accel(accelIn), - gyro(gyroIn) - { } - + vn::math::vec3f yawPitchRoll; ///< The yawPitchRoll field. + vn::math::vec3f mag; ///< The mag field. + vn::math::vec3f accel; ///< The accel field. + vn::math::vec3f gyro; ///< The gyro field. + + YawPitchRollMagneticAccelerationAndAngularRatesRegister() {} + + /// \brief Creates an initializes a new YawPitchRollMagneticAccelerationAndAngularRatesRegister structure. + /// + /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. + /// \param[in] magIn Value to initialize the mag field with. + /// \param[in] accelIn Value to initialize the accel field with. + /// \param[in] gyroIn Value to initialize the gyro field with. + YawPitchRollMagneticAccelerationAndAngularRatesRegister( + vn::math::vec3f yawPitchRollIn, vn::math::vec3f magIn, vn::math::vec3f accelIn, + vn::math::vec3f gyroIn) + : yawPitchRoll(yawPitchRollIn), mag(magIn), accel(accelIn), gyro(gyroIn) + { + } }; /// \brief Structure representing the Communication Protocol Control register. struct CommunicationProtocolControlRegister { - protocol::uart::CountMode serialCount; ///< The serialCount field. - protocol::uart::StatusMode serialStatus; ///< The serialStatus field. - protocol::uart::CountMode spiCount; ///< The spiCount field. - protocol::uart::StatusMode spiStatus; ///< The spiStatus field. - protocol::uart::ChecksumMode serialChecksum; ///< The serialChecksum field. - protocol::uart::ChecksumMode spiChecksum; ///< The spiChecksum field. - protocol::uart::ErrorMode errorMode; ///< The errorMode field. - - CommunicationProtocolControlRegister() { } - - /// \brief Creates an initializes a new CommunicationProtocolControlRegister structure. - /// - /// \param[in] serialCountIn Value to initialize the serialCount field with. - /// \param[in] serialStatusIn Value to initialize the serialStatus field with. - /// \param[in] spiCountIn Value to initialize the spiCount field with. - /// \param[in] spiStatusIn Value to initialize the spiStatus field with. - /// \param[in] serialChecksumIn Value to initialize the serialChecksum field with. - /// \param[in] spiChecksumIn Value to initialize the spiChecksum field with. - /// \param[in] errorModeIn Value to initialize the errorMode field with. - CommunicationProtocolControlRegister( - protocol::uart::CountMode serialCountIn, - protocol::uart::StatusMode serialStatusIn, - protocol::uart::CountMode spiCountIn, - protocol::uart::StatusMode spiStatusIn, - protocol::uart::ChecksumMode serialChecksumIn, - protocol::uart::ChecksumMode spiChecksumIn, - protocol::uart::ErrorMode errorModeIn) : - serialCount(serialCountIn), - serialStatus(serialStatusIn), - spiCount(spiCountIn), - spiStatus(spiStatusIn), - serialChecksum(serialChecksumIn), - spiChecksum(spiChecksumIn), - errorMode(errorModeIn) - { } - + protocol::uart::CountMode serialCount; ///< The serialCount field. + protocol::uart::StatusMode serialStatus; ///< The serialStatus field. + protocol::uart::CountMode spiCount; ///< The spiCount field. + protocol::uart::StatusMode spiStatus; ///< The spiStatus field. + protocol::uart::ChecksumMode serialChecksum; ///< The serialChecksum field. + protocol::uart::ChecksumMode spiChecksum; ///< The spiChecksum field. + protocol::uart::ErrorMode errorMode; ///< The errorMode field. + + CommunicationProtocolControlRegister() {} + + /// \brief Creates an initializes a new CommunicationProtocolControlRegister structure. + /// + /// \param[in] serialCountIn Value to initialize the serialCount field with. + /// \param[in] serialStatusIn Value to initialize the serialStatus field with. + /// \param[in] spiCountIn Value to initialize the spiCount field with. + /// \param[in] spiStatusIn Value to initialize the spiStatus field with. + /// \param[in] serialChecksumIn Value to initialize the serialChecksum field with. + /// \param[in] spiChecksumIn Value to initialize the spiChecksum field with. + /// \param[in] errorModeIn Value to initialize the errorMode field with. + CommunicationProtocolControlRegister( + protocol::uart::CountMode serialCountIn, protocol::uart::StatusMode serialStatusIn, + protocol::uart::CountMode spiCountIn, protocol::uart::StatusMode spiStatusIn, + protocol::uart::ChecksumMode serialChecksumIn, protocol::uart::ChecksumMode spiChecksumIn, + protocol::uart::ErrorMode errorModeIn) + : serialCount(serialCountIn), + serialStatus(serialStatusIn), + spiCount(spiCountIn), + spiStatus(spiStatusIn), + serialChecksum(serialChecksumIn), + spiChecksum(spiChecksumIn), + errorMode(errorModeIn) + { + } }; /// \brief Structure representing the Synchronization Control register. struct SynchronizationControlRegister { - protocol::uart::SyncInMode syncInMode; ///< The syncInMode field. - protocol::uart::SyncInEdge syncInEdge; ///< The syncInEdge field. - uint16_t syncInSkipFactor; ///< The syncInSkipFactor field. - protocol::uart::SyncOutMode syncOutMode; ///< The syncOutMode field. - protocol::uart::SyncOutPolarity syncOutPolarity; ///< The syncOutPolarity field. - uint16_t syncOutSkipFactor; ///< The syncOutSkipFactor field. - uint32_t syncOutPulseWidth; ///< The syncOutPulseWidth field. - - SynchronizationControlRegister() { } - - /// \brief Creates an initializes a new SynchronizationControlRegister structure. - /// - /// \param[in] syncInModeIn Value to initialize the syncInMode field with. - /// \param[in] syncInEdgeIn Value to initialize the syncInEdge field with. - /// \param[in] syncInSkipFactorIn Value to initialize the syncInSkipFactor field with. - /// \param[in] syncOutModeIn Value to initialize the syncOutMode field with. - /// \param[in] syncOutPolarityIn Value to initialize the syncOutPolarity field with. - /// \param[in] syncOutSkipFactorIn Value to initialize the syncOutSkipFactor field with. - /// \param[in] syncOutPulseWidthIn Value to initialize the syncOutPulseWidth field with. - SynchronizationControlRegister( - protocol::uart::SyncInMode syncInModeIn, - protocol::uart::SyncInEdge syncInEdgeIn, - uint16_t syncInSkipFactorIn, - protocol::uart::SyncOutMode syncOutModeIn, - protocol::uart::SyncOutPolarity syncOutPolarityIn, - uint16_t syncOutSkipFactorIn, - uint32_t syncOutPulseWidthIn) : - syncInMode(syncInModeIn), - syncInEdge(syncInEdgeIn), - syncInSkipFactor(syncInSkipFactorIn), - syncOutMode(syncOutModeIn), - syncOutPolarity(syncOutPolarityIn), - syncOutSkipFactor(syncOutSkipFactorIn), - syncOutPulseWidth(syncOutPulseWidthIn) - { } - + protocol::uart::SyncInMode syncInMode; ///< The syncInMode field. + protocol::uart::SyncInEdge syncInEdge; ///< The syncInEdge field. + uint16_t syncInSkipFactor; ///< The syncInSkipFactor field. + protocol::uart::SyncOutMode syncOutMode; ///< The syncOutMode field. + protocol::uart::SyncOutPolarity syncOutPolarity; ///< The syncOutPolarity field. + uint16_t syncOutSkipFactor; ///< The syncOutSkipFactor field. + uint32_t syncOutPulseWidth; ///< The syncOutPulseWidth field. + + SynchronizationControlRegister() {} + + /// \brief Creates an initializes a new SynchronizationControlRegister structure. + /// + /// \param[in] syncInModeIn Value to initialize the syncInMode field with. + /// \param[in] syncInEdgeIn Value to initialize the syncInEdge field with. + /// \param[in] syncInSkipFactorIn Value to initialize the syncInSkipFactor field with. + /// \param[in] syncOutModeIn Value to initialize the syncOutMode field with. + /// \param[in] syncOutPolarityIn Value to initialize the syncOutPolarity field with. + /// \param[in] syncOutSkipFactorIn Value to initialize the syncOutSkipFactor field with. + /// \param[in] syncOutPulseWidthIn Value to initialize the syncOutPulseWidth field with. + SynchronizationControlRegister( + protocol::uart::SyncInMode syncInModeIn, protocol::uart::SyncInEdge syncInEdgeIn, + uint16_t syncInSkipFactorIn, protocol::uart::SyncOutMode syncOutModeIn, + protocol::uart::SyncOutPolarity syncOutPolarityIn, uint16_t syncOutSkipFactorIn, + uint32_t syncOutPulseWidthIn) + : syncInMode(syncInModeIn), + syncInEdge(syncInEdgeIn), + syncInSkipFactor(syncInSkipFactorIn), + syncOutMode(syncOutModeIn), + syncOutPolarity(syncOutPolarityIn), + syncOutSkipFactor(syncOutSkipFactorIn), + syncOutPulseWidth(syncOutPulseWidthIn) + { + } }; /// \brief Structure representing the Synchronization Status register. struct SynchronizationStatusRegister { - uint32_t syncInCount; ///< The syncInCount field. - uint32_t syncInTime; ///< The syncInTime field. - uint32_t syncOutCount; ///< The syncOutCount field. - - SynchronizationStatusRegister() { } - - /// \brief Creates an initializes a new SynchronizationStatusRegister structure. - /// - /// \param[in] syncInCountIn Value to initialize the syncInCount field with. - /// \param[in] syncInTimeIn Value to initialize the syncInTime field with. - /// \param[in] syncOutCountIn Value to initialize the syncOutCount field with. - SynchronizationStatusRegister( - uint32_t syncInCountIn, - uint32_t syncInTimeIn, - uint32_t syncOutCountIn) : - syncInCount(syncInCountIn), - syncInTime(syncInTimeIn), - syncOutCount(syncOutCountIn) - { } - + uint32_t syncInCount; ///< The syncInCount field. + uint32_t syncInTime; ///< The syncInTime field. + uint32_t syncOutCount; ///< The syncOutCount field. + + SynchronizationStatusRegister() {} + + /// \brief Creates an initializes a new SynchronizationStatusRegister structure. + /// + /// \param[in] syncInCountIn Value to initialize the syncInCount field with. + /// \param[in] syncInTimeIn Value to initialize the syncInTime field with. + /// \param[in] syncOutCountIn Value to initialize the syncOutCount field with. + SynchronizationStatusRegister( + uint32_t syncInCountIn, uint32_t syncInTimeIn, uint32_t syncOutCountIn) + : syncInCount(syncInCountIn), syncInTime(syncInTimeIn), syncOutCount(syncOutCountIn) + { + } }; /// \brief Structure representing the Filter Basic Control register. struct FilterBasicControlRegister { - protocol::uart::MagneticMode magMode; ///< The magMode field. - protocol::uart::ExternalSensorMode extMagMode; ///< The extMagMode field. - protocol::uart::ExternalSensorMode extAccMode; ///< The extAccMode field. - protocol::uart::ExternalSensorMode extGyroMode; ///< The extGyroMode field. - vn::math::vec3f gyroLimit; ///< The gyroLimit field. - - FilterBasicControlRegister() { } - - /// \brief Creates an initializes a new FilterBasicControlRegister structure. - /// - /// \param[in] magModeIn Value to initialize the magMode field with. - /// \param[in] extMagModeIn Value to initialize the extMagMode field with. - /// \param[in] extAccModeIn Value to initialize the extAccMode field with. - /// \param[in] extGyroModeIn Value to initialize the extGyroMode field with. - /// \param[in] gyroLimitIn Value to initialize the gyroLimit field with. - FilterBasicControlRegister( - protocol::uart::MagneticMode magModeIn, - protocol::uart::ExternalSensorMode extMagModeIn, - protocol::uart::ExternalSensorMode extAccModeIn, - protocol::uart::ExternalSensorMode extGyroModeIn, - vn::math::vec3f gyroLimitIn) : - magMode(magModeIn), - extMagMode(extMagModeIn), - extAccMode(extAccModeIn), - extGyroMode(extGyroModeIn), - gyroLimit(gyroLimitIn) - { } - + protocol::uart::MagneticMode magMode; ///< The magMode field. + protocol::uart::ExternalSensorMode extMagMode; ///< The extMagMode field. + protocol::uart::ExternalSensorMode extAccMode; ///< The extAccMode field. + protocol::uart::ExternalSensorMode extGyroMode; ///< The extGyroMode field. + vn::math::vec3f gyroLimit; ///< The gyroLimit field. + + FilterBasicControlRegister() {} + + /// \brief Creates an initializes a new FilterBasicControlRegister structure. + /// + /// \param[in] magModeIn Value to initialize the magMode field with. + /// \param[in] extMagModeIn Value to initialize the extMagMode field with. + /// \param[in] extAccModeIn Value to initialize the extAccMode field with. + /// \param[in] extGyroModeIn Value to initialize the extGyroMode field with. + /// \param[in] gyroLimitIn Value to initialize the gyroLimit field with. + FilterBasicControlRegister( + protocol::uart::MagneticMode magModeIn, protocol::uart::ExternalSensorMode extMagModeIn, + protocol::uart::ExternalSensorMode extAccModeIn, + protocol::uart::ExternalSensorMode extGyroModeIn, vn::math::vec3f gyroLimitIn) + : magMode(magModeIn), + extMagMode(extMagModeIn), + extAccMode(extAccModeIn), + extGyroMode(extGyroModeIn), + gyroLimit(gyroLimitIn) + { + } }; /// \brief Structure representing the VPE Basic Control register. struct VpeBasicControlRegister { - protocol::uart::VpeEnable enable; ///< The enable field. - protocol::uart::HeadingMode headingMode; ///< The headingMode field. - protocol::uart::VpeMode filteringMode; ///< The filteringMode field. - protocol::uart::VpeMode tuningMode; ///< The tuningMode field. - - VpeBasicControlRegister() { } - - /// \brief Creates an initializes a new VpeBasicControlRegister structure. - /// - /// \param[in] enableIn Value to initialize the enable field with. - /// \param[in] headingModeIn Value to initialize the headingMode field with. - /// \param[in] filteringModeIn Value to initialize the filteringMode field with. - /// \param[in] tuningModeIn Value to initialize the tuningMode field with. - VpeBasicControlRegister( - protocol::uart::VpeEnable enableIn, - protocol::uart::HeadingMode headingModeIn, - protocol::uart::VpeMode filteringModeIn, - protocol::uart::VpeMode tuningModeIn) : - enable(enableIn), - headingMode(headingModeIn), - filteringMode(filteringModeIn), - tuningMode(tuningModeIn) - { } - + protocol::uart::VpeEnable enable; ///< The enable field. + protocol::uart::HeadingMode headingMode; ///< The headingMode field. + protocol::uart::VpeMode filteringMode; ///< The filteringMode field. + protocol::uart::VpeMode tuningMode; ///< The tuningMode field. + + VpeBasicControlRegister() {} + + /// \brief Creates an initializes a new VpeBasicControlRegister structure. + /// + /// \param[in] enableIn Value to initialize the enable field with. + /// \param[in] headingModeIn Value to initialize the headingMode field with. + /// \param[in] filteringModeIn Value to initialize the filteringMode field with. + /// \param[in] tuningModeIn Value to initialize the tuningMode field with. + VpeBasicControlRegister( + protocol::uart::VpeEnable enableIn, protocol::uart::HeadingMode headingModeIn, + protocol::uart::VpeMode filteringModeIn, protocol::uart::VpeMode tuningModeIn) + : enable(enableIn), + headingMode(headingModeIn), + filteringMode(filteringModeIn), + tuningMode(tuningModeIn) + { + } }; /// \brief Structure representing the Heave Configuration Register struct HeaveConfigurationRegister { - float initialWavePeriod; - float initialWaveAmplitude; - float maxWavePeriod; - float minWaveAmplitude; - float delayedHeaveCutoffFreq; - float heaveCutoffFreq; - float heaveRateCutoffFreq; - - HeaveConfigurationRegister() { } - - /// \brief Creates and initializes a new HeaveConfigurationRegister structure. - /// - /// \param[in] initialWavePeriodIn Value to initialize the initialWavePeriod field with. - /// \param[in] initialWaveAmplitudeIn Value to initialize the initialWaveAmplitude field with. - /// \param[in] maxWavePeriodIn Value to initialize the maxWavePeriod field with. - /// \param[in] minWaveAmplitudeIn Value to initialize the minWaveAmplitude field with. - /// \param[in] delayedHeaveCutoffFreqIn Value to initialize the delayedHeaveCutoffFreq field with. - /// \param[in] heaveCutoffFreqIn Value to initialize the heaveCutoffFreq field with. - /// \param[in] heaveRateCutoffFreqIn Value to initialize the heaveRateCutoffFreq field with. - HeaveConfigurationRegister( - float initialWavePeriodIn, - float initialWaveAmplitudeIn, - float maxWavePeriodIn, - float minWaveAmplitudeIn, - float delayedHeaveCutoffFreqIn, - float heaveCutoffFreqIn, - float heaveRateCutoffFreqIn - ) : - initialWavePeriod(initialWavePeriodIn), - initialWaveAmplitude(initialWaveAmplitudeIn), - maxWavePeriod(maxWavePeriodIn), - minWaveAmplitude(minWaveAmplitudeIn), - delayedHeaveCutoffFreq(delayedHeaveCutoffFreqIn), - heaveCutoffFreq(heaveCutoffFreqIn), - heaveRateCutoffFreq(heaveRateCutoffFreqIn) - { } + float initialWavePeriod; + float initialWaveAmplitude; + float maxWavePeriod; + float minWaveAmplitude; + float delayedHeaveCutoffFreq; + float heaveCutoffFreq; + float heaveRateCutoffFreq; + + HeaveConfigurationRegister() {} + + /// \brief Creates and initializes a new HeaveConfigurationRegister structure. + /// + /// \param[in] initialWavePeriodIn Value to initialize the initialWavePeriod field with. + /// \param[in] initialWaveAmplitudeIn Value to initialize the initialWaveAmplitude field with. + /// \param[in] maxWavePeriodIn Value to initialize the maxWavePeriod field with. + /// \param[in] minWaveAmplitudeIn Value to initialize the minWaveAmplitude field with. + /// \param[in] delayedHeaveCutoffFreqIn Value to initialize the delayedHeaveCutoffFreq field with. + /// \param[in] heaveCutoffFreqIn Value to initialize the heaveCutoffFreq field with. + /// \param[in] heaveRateCutoffFreqIn Value to initialize the heaveRateCutoffFreq field with. + HeaveConfigurationRegister( + float initialWavePeriodIn, float initialWaveAmplitudeIn, float maxWavePeriodIn, + float minWaveAmplitudeIn, float delayedHeaveCutoffFreqIn, float heaveCutoffFreqIn, + float heaveRateCutoffFreqIn) + : initialWavePeriod(initialWavePeriodIn), + initialWaveAmplitude(initialWaveAmplitudeIn), + maxWavePeriod(maxWavePeriodIn), + minWaveAmplitude(minWaveAmplitudeIn), + delayedHeaveCutoffFreq(delayedHeaveCutoffFreqIn), + heaveCutoffFreq(heaveCutoffFreqIn), + heaveRateCutoffFreq(heaveRateCutoffFreqIn) + { + } }; /// \brief Structure representing the VPE Magnetometer Basic Tuning register. struct VpeMagnetometerBasicTuningRegister { - vn::math::vec3f baseTuning; ///< The baseTuning field. - vn::math::vec3f adaptiveTuning; ///< The adaptiveTuning field. - vn::math::vec3f adaptiveFiltering; ///< The adaptiveFiltering field. - - VpeMagnetometerBasicTuningRegister() { } - - /// \brief Creates an initializes a new VpeMagnetometerBasicTuningRegister structure. - /// - /// \param[in] baseTuningIn Value to initialize the baseTuning field with. - /// \param[in] adaptiveTuningIn Value to initialize the adaptiveTuning field with. - /// \param[in] adaptiveFilteringIn Value to initialize the adaptiveFiltering field with. - VpeMagnetometerBasicTuningRegister( - vn::math::vec3f baseTuningIn, - vn::math::vec3f adaptiveTuningIn, - vn::math::vec3f adaptiveFilteringIn) : - baseTuning(baseTuningIn), - adaptiveTuning(adaptiveTuningIn), - adaptiveFiltering(adaptiveFilteringIn) - { } - + vn::math::vec3f baseTuning; ///< The baseTuning field. + vn::math::vec3f adaptiveTuning; ///< The adaptiveTuning field. + vn::math::vec3f adaptiveFiltering; ///< The adaptiveFiltering field. + + VpeMagnetometerBasicTuningRegister() {} + + /// \brief Creates an initializes a new VpeMagnetometerBasicTuningRegister structure. + /// + /// \param[in] baseTuningIn Value to initialize the baseTuning field with. + /// \param[in] adaptiveTuningIn Value to initialize the adaptiveTuning field with. + /// \param[in] adaptiveFilteringIn Value to initialize the adaptiveFiltering field with. + VpeMagnetometerBasicTuningRegister( + vn::math::vec3f baseTuningIn, vn::math::vec3f adaptiveTuningIn, + vn::math::vec3f adaptiveFilteringIn) + : baseTuning(baseTuningIn), + adaptiveTuning(adaptiveTuningIn), + adaptiveFiltering(adaptiveFilteringIn) + { + } }; /// \brief Structure representing the VPE Magnetometer Advanced Tuning register. struct VpeMagnetometerAdvancedTuningRegister { - vn::math::vec3f minFiltering; ///< The minFiltering field. - vn::math::vec3f maxFiltering; ///< The maxFiltering field. - float maxAdaptRate; ///< The maxAdaptRate field. - float disturbanceWindow; ///< The disturbanceWindow field. - float maxTuning; ///< The maxTuning field. - - VpeMagnetometerAdvancedTuningRegister() { } - - /// \brief Creates an initializes a new VpeMagnetometerAdvancedTuningRegister structure. - /// - /// \param[in] minFilteringIn Value to initialize the minFiltering field with. - /// \param[in] maxFilteringIn Value to initialize the maxFiltering field with. - /// \param[in] maxAdaptRateIn Value to initialize the maxAdaptRate field with. - /// \param[in] disturbanceWindowIn Value to initialize the disturbanceWindow field with. - /// \param[in] maxTuningIn Value to initialize the maxTuning field with. - VpeMagnetometerAdvancedTuningRegister( - vn::math::vec3f minFilteringIn, - vn::math::vec3f maxFilteringIn, - float maxAdaptRateIn, - float disturbanceWindowIn, - float maxTuningIn) : - minFiltering(minFilteringIn), - maxFiltering(maxFilteringIn), - maxAdaptRate(maxAdaptRateIn), - disturbanceWindow(disturbanceWindowIn), - maxTuning(maxTuningIn) - { } - + vn::math::vec3f minFiltering; ///< The minFiltering field. + vn::math::vec3f maxFiltering; ///< The maxFiltering field. + float maxAdaptRate; ///< The maxAdaptRate field. + float disturbanceWindow; ///< The disturbanceWindow field. + float maxTuning; ///< The maxTuning field. + + VpeMagnetometerAdvancedTuningRegister() {} + + /// \brief Creates an initializes a new VpeMagnetometerAdvancedTuningRegister structure. + /// + /// \param[in] minFilteringIn Value to initialize the minFiltering field with. + /// \param[in] maxFilteringIn Value to initialize the maxFiltering field with. + /// \param[in] maxAdaptRateIn Value to initialize the maxAdaptRate field with. + /// \param[in] disturbanceWindowIn Value to initialize the disturbanceWindow field with. + /// \param[in] maxTuningIn Value to initialize the maxTuning field with. + VpeMagnetometerAdvancedTuningRegister( + vn::math::vec3f minFilteringIn, vn::math::vec3f maxFilteringIn, float maxAdaptRateIn, + float disturbanceWindowIn, float maxTuningIn) + : minFiltering(minFilteringIn), + maxFiltering(maxFilteringIn), + maxAdaptRate(maxAdaptRateIn), + disturbanceWindow(disturbanceWindowIn), + maxTuning(maxTuningIn) + { + } }; /// \brief Structure representing the VPE Accelerometer Basic Tuning register. struct VpeAccelerometerBasicTuningRegister { - vn::math::vec3f baseTuning; ///< The baseTuning field. - vn::math::vec3f adaptiveTuning; ///< The adaptiveTuning field. - vn::math::vec3f adaptiveFiltering; ///< The adaptiveFiltering field. - - VpeAccelerometerBasicTuningRegister() { } - - /// \brief Creates an initializes a new VpeAccelerometerBasicTuningRegister structure. - /// - /// \param[in] baseTuningIn Value to initialize the baseTuning field with. - /// \param[in] adaptiveTuningIn Value to initialize the adaptiveTuning field with. - /// \param[in] adaptiveFilteringIn Value to initialize the adaptiveFiltering field with. - VpeAccelerometerBasicTuningRegister( - vn::math::vec3f baseTuningIn, - vn::math::vec3f adaptiveTuningIn, - vn::math::vec3f adaptiveFilteringIn) : - baseTuning(baseTuningIn), - adaptiveTuning(adaptiveTuningIn), - adaptiveFiltering(adaptiveFilteringIn) - { } - + vn::math::vec3f baseTuning; ///< The baseTuning field. + vn::math::vec3f adaptiveTuning; ///< The adaptiveTuning field. + vn::math::vec3f adaptiveFiltering; ///< The adaptiveFiltering field. + + VpeAccelerometerBasicTuningRegister() {} + + /// \brief Creates an initializes a new VpeAccelerometerBasicTuningRegister structure. + /// + /// \param[in] baseTuningIn Value to initialize the baseTuning field with. + /// \param[in] adaptiveTuningIn Value to initialize the adaptiveTuning field with. + /// \param[in] adaptiveFilteringIn Value to initialize the adaptiveFiltering field with. + VpeAccelerometerBasicTuningRegister( + vn::math::vec3f baseTuningIn, vn::math::vec3f adaptiveTuningIn, + vn::math::vec3f adaptiveFilteringIn) + : baseTuning(baseTuningIn), + adaptiveTuning(adaptiveTuningIn), + adaptiveFiltering(adaptiveFilteringIn) + { + } }; /// \brief Structure representing the VPE Accelerometer Advanced Tuning register. struct VpeAccelerometerAdvancedTuningRegister { - vn::math::vec3f minFiltering; ///< The minFiltering field. - vn::math::vec3f maxFiltering; ///< The maxFiltering field. - float maxAdaptRate; ///< The maxAdaptRate field. - float disturbanceWindow; ///< The disturbanceWindow field. - float maxTuning; ///< The maxTuning field. - - VpeAccelerometerAdvancedTuningRegister() { } - - /// \brief Creates an initializes a new VpeAccelerometerAdvancedTuningRegister structure. - /// - /// \param[in] minFilteringIn Value to initialize the minFiltering field with. - /// \param[in] maxFilteringIn Value to initialize the maxFiltering field with. - /// \param[in] maxAdaptRateIn Value to initialize the maxAdaptRate field with. - /// \param[in] disturbanceWindowIn Value to initialize the disturbanceWindow field with. - /// \param[in] maxTuningIn Value to initialize the maxTuning field with. - VpeAccelerometerAdvancedTuningRegister( - vn::math::vec3f minFilteringIn, - vn::math::vec3f maxFilteringIn, - float maxAdaptRateIn, - float disturbanceWindowIn, - float maxTuningIn) : - minFiltering(minFilteringIn), - maxFiltering(maxFilteringIn), - maxAdaptRate(maxAdaptRateIn), - disturbanceWindow(disturbanceWindowIn), - maxTuning(maxTuningIn) - { } - + vn::math::vec3f minFiltering; ///< The minFiltering field. + vn::math::vec3f maxFiltering; ///< The maxFiltering field. + float maxAdaptRate; ///< The maxAdaptRate field. + float disturbanceWindow; ///< The disturbanceWindow field. + float maxTuning; ///< The maxTuning field. + + VpeAccelerometerAdvancedTuningRegister() {} + + /// \brief Creates an initializes a new VpeAccelerometerAdvancedTuningRegister structure. + /// + /// \param[in] minFilteringIn Value to initialize the minFiltering field with. + /// \param[in] maxFilteringIn Value to initialize the maxFiltering field with. + /// \param[in] maxAdaptRateIn Value to initialize the maxAdaptRate field with. + /// \param[in] disturbanceWindowIn Value to initialize the disturbanceWindow field with. + /// \param[in] maxTuningIn Value to initialize the maxTuning field with. + VpeAccelerometerAdvancedTuningRegister( + vn::math::vec3f minFilteringIn, vn::math::vec3f maxFilteringIn, float maxAdaptRateIn, + float disturbanceWindowIn, float maxTuningIn) + : minFiltering(minFilteringIn), + maxFiltering(maxFilteringIn), + maxAdaptRate(maxAdaptRateIn), + disturbanceWindow(disturbanceWindowIn), + maxTuning(maxTuningIn) + { + } }; /// \brief Structure representing the VPE Gyro Basic Tuning register. struct VpeGyroBasicTuningRegister { - vn::math::vec3f angularWalkVariance; ///< The angularWalkVariance field. - vn::math::vec3f baseTuning; ///< The baseTuning field. - vn::math::vec3f adaptiveTuning; ///< The adaptiveTuning field. - - VpeGyroBasicTuningRegister() { } - - /// \brief Creates an initializes a new VpeGyroBasicTuningRegister structure. - /// - /// \param[in] angularWalkVarianceIn Value to initialize the angularWalkVariance field with. - /// \param[in] baseTuningIn Value to initialize the baseTuning field with. - /// \param[in] adaptiveTuningIn Value to initialize the adaptiveTuning field with. - VpeGyroBasicTuningRegister( - vn::math::vec3f angularWalkVarianceIn, - vn::math::vec3f baseTuningIn, - vn::math::vec3f adaptiveTuningIn) : - angularWalkVariance(angularWalkVarianceIn), - baseTuning(baseTuningIn), - adaptiveTuning(adaptiveTuningIn) - { } - + vn::math::vec3f angularWalkVariance; ///< The angularWalkVariance field. + vn::math::vec3f baseTuning; ///< The baseTuning field. + vn::math::vec3f adaptiveTuning; ///< The adaptiveTuning field. + + VpeGyroBasicTuningRegister() {} + + /// \brief Creates an initializes a new VpeGyroBasicTuningRegister structure. + /// + /// \param[in] angularWalkVarianceIn Value to initialize the angularWalkVariance field with. + /// \param[in] baseTuningIn Value to initialize the baseTuning field with. + /// \param[in] adaptiveTuningIn Value to initialize the adaptiveTuning field with. + VpeGyroBasicTuningRegister( + vn::math::vec3f angularWalkVarianceIn, vn::math::vec3f baseTuningIn, + vn::math::vec3f adaptiveTuningIn) + : angularWalkVariance(angularWalkVarianceIn), + baseTuning(baseTuningIn), + adaptiveTuning(adaptiveTuningIn) + { + } }; /// \brief Structure representing the Magnetometer Calibration Control register. struct MagnetometerCalibrationControlRegister { - protocol::uart::HsiMode hsiMode; ///< The hsiMode field. - protocol::uart::HsiOutput hsiOutput; ///< The hsiOutput field. - uint8_t convergeRate; ///< The convergeRate field. - - MagnetometerCalibrationControlRegister() { } - - /// \brief Creates an initializes a new MagnetometerCalibrationControlRegister structure. - /// - /// \param[in] hsiModeIn Value to initialize the hsiMode field with. - /// \param[in] hsiOutputIn Value to initialize the hsiOutput field with. - /// \param[in] convergeRateIn Value to initialize the convergeRate field with. - MagnetometerCalibrationControlRegister( - protocol::uart::HsiMode hsiModeIn, - protocol::uart::HsiOutput hsiOutputIn, - uint8_t convergeRateIn) : - hsiMode(hsiModeIn), - hsiOutput(hsiOutputIn), - convergeRate(convergeRateIn) - { } - + protocol::uart::HsiMode hsiMode; ///< The hsiMode field. + protocol::uart::HsiOutput hsiOutput; ///< The hsiOutput field. + uint8_t convergeRate; ///< The convergeRate field. + + MagnetometerCalibrationControlRegister() {} + + /// \brief Creates an initializes a new MagnetometerCalibrationControlRegister structure. + /// + /// \param[in] hsiModeIn Value to initialize the hsiMode field with. + /// \param[in] hsiOutputIn Value to initialize the hsiOutput field with. + /// \param[in] convergeRateIn Value to initialize the convergeRate field with. + MagnetometerCalibrationControlRegister( + protocol::uart::HsiMode hsiModeIn, protocol::uart::HsiOutput hsiOutputIn, + uint8_t convergeRateIn) + : hsiMode(hsiModeIn), hsiOutput(hsiOutputIn), convergeRate(convergeRateIn) + { + } }; /// \brief Structure representing the Calculated Magnetometer Calibration register. struct CalculatedMagnetometerCalibrationRegister { - vn::math::mat3f c; ///< The c field. - vn::math::vec3f b; ///< The b field. - - CalculatedMagnetometerCalibrationRegister() { } - - /// \brief Creates an initializes a new CalculatedMagnetometerCalibrationRegister structure. - /// - /// \param[in] cIn Value to initialize the c field with. - /// \param[in] bIn Value to initialize the b field with. - CalculatedMagnetometerCalibrationRegister( - vn::math::mat3f cIn, - vn::math::vec3f bIn) : - c(cIn), - b(bIn) - { } - + vn::math::mat3f c; ///< The c field. + vn::math::vec3f b; ///< The b field. + + CalculatedMagnetometerCalibrationRegister() {} + + /// \brief Creates an initializes a new CalculatedMagnetometerCalibrationRegister structure. + /// + /// \param[in] cIn Value to initialize the c field with. + /// \param[in] bIn Value to initialize the b field with. + CalculatedMagnetometerCalibrationRegister(vn::math::mat3f cIn, vn::math::vec3f bIn) + : c(cIn), b(bIn) + { + } }; /// \brief Structure representing the Velocity Compensation Control register. struct VelocityCompensationControlRegister { - protocol::uart::VelocityCompensationMode mode; ///< The mode field. - float velocityTuning; ///< The velocityTuning field. - float rateTuning; ///< The rateTuning field. - - VelocityCompensationControlRegister() { } - - /// \brief Creates an initializes a new VelocityCompensationControlRegister structure. - /// - /// \param[in] modeIn Value to initialize the mode field with. - /// \param[in] velocityTuningIn Value to initialize the velocityTuning field with. - /// \param[in] rateTuningIn Value to initialize the rateTuning field with. - VelocityCompensationControlRegister( - protocol::uart::VelocityCompensationMode modeIn, - float velocityTuningIn, - float rateTuningIn) : - mode(modeIn), - velocityTuning(velocityTuningIn), - rateTuning(rateTuningIn) - { } - + protocol::uart::VelocityCompensationMode mode; ///< The mode field. + float velocityTuning; ///< The velocityTuning field. + float rateTuning; ///< The rateTuning field. + + VelocityCompensationControlRegister() {} + + /// \brief Creates an initializes a new VelocityCompensationControlRegister structure. + /// + /// \param[in] modeIn Value to initialize the mode field with. + /// \param[in] velocityTuningIn Value to initialize the velocityTuning field with. + /// \param[in] rateTuningIn Value to initialize the rateTuning field with. + VelocityCompensationControlRegister( + protocol::uart::VelocityCompensationMode modeIn, float velocityTuningIn, float rateTuningIn) + : mode(modeIn), velocityTuning(velocityTuningIn), rateTuning(rateTuningIn) + { + } }; /// \brief Structure representing the Velocity Compensation Status register. struct VelocityCompensationStatusRegister { - float x; ///< The x field. - float xDot; ///< The xDot field. - vn::math::vec3f accelOffset; ///< The accelOffset field. - vn::math::vec3f omega; ///< The omega field. - - VelocityCompensationStatusRegister() { } - - /// \brief Creates an initializes a new VelocityCompensationStatusRegister structure. - /// - /// \param[in] xIn Value to initialize the x field with. - /// \param[in] xDotIn Value to initialize the xDot field with. - /// \param[in] accelOffsetIn Value to initialize the accelOffset field with. - /// \param[in] omegaIn Value to initialize the omega field with. - VelocityCompensationStatusRegister( - float xIn, - float xDotIn, - vn::math::vec3f accelOffsetIn, - vn::math::vec3f omegaIn) : - x(xIn), - xDot(xDotIn), - accelOffset(accelOffsetIn), - omega(omegaIn) - { } - + float x; ///< The x field. + float xDot; ///< The xDot field. + vn::math::vec3f accelOffset; ///< The accelOffset field. + vn::math::vec3f omega; ///< The omega field. + + VelocityCompensationStatusRegister() {} + + /// \brief Creates an initializes a new VelocityCompensationStatusRegister structure. + /// + /// \param[in] xIn Value to initialize the x field with. + /// \param[in] xDotIn Value to initialize the xDot field with. + /// \param[in] accelOffsetIn Value to initialize the accelOffset field with. + /// \param[in] omegaIn Value to initialize the omega field with. + VelocityCompensationStatusRegister( + float xIn, float xDotIn, vn::math::vec3f accelOffsetIn, vn::math::vec3f omegaIn) + : x(xIn), xDot(xDotIn), accelOffset(accelOffsetIn), omega(omegaIn) + { + } }; /// \brief Structure representing the IMU Measurements register. struct ImuMeasurementsRegister { - vn::math::vec3f mag; ///< The mag field. - vn::math::vec3f accel; ///< The accel field. - vn::math::vec3f gyro; ///< The gyro field. - float temp; ///< The temp field. - float pressure; ///< The pressure field. - - ImuMeasurementsRegister() { } - - /// \brief Creates an initializes a new ImuMeasurementsRegister structure. - /// - /// \param[in] magIn Value to initialize the mag field with. - /// \param[in] accelIn Value to initialize the accel field with. - /// \param[in] gyroIn Value to initialize the gyro field with. - /// \param[in] tempIn Value to initialize the temp field with. - /// \param[in] pressureIn Value to initialize the pressure field with. - ImuMeasurementsRegister( - vn::math::vec3f magIn, - vn::math::vec3f accelIn, - vn::math::vec3f gyroIn, - float tempIn, - float pressureIn) : - mag(magIn), - accel(accelIn), - gyro(gyroIn), - temp(tempIn), - pressure(pressureIn) - { } - + vn::math::vec3f mag; ///< The mag field. + vn::math::vec3f accel; ///< The accel field. + vn::math::vec3f gyro; ///< The gyro field. + float temp; ///< The temp field. + float pressure; ///< The pressure field. + + ImuMeasurementsRegister() {} + + /// \brief Creates an initializes a new ImuMeasurementsRegister structure. + /// + /// \param[in] magIn Value to initialize the mag field with. + /// \param[in] accelIn Value to initialize the accel field with. + /// \param[in] gyroIn Value to initialize the gyro field with. + /// \param[in] tempIn Value to initialize the temp field with. + /// \param[in] pressureIn Value to initialize the pressure field with. + ImuMeasurementsRegister( + vn::math::vec3f magIn, vn::math::vec3f accelIn, vn::math::vec3f gyroIn, float tempIn, + float pressureIn) + : mag(magIn), accel(accelIn), gyro(gyroIn), temp(tempIn), pressure(pressureIn) + { + } }; /// \brief Structure representing the GPS Configuration register. struct GpsConfigurationRegister { - protocol::uart::GpsMode mode; ///< The mode field. - protocol::uart::PpsSource ppsSource; ///< The ppsSource field. - protocol::uart::GpsRate rate; ///< The rate field. - protocol::uart::AntPower antPow; ///< The antPow field. - - GpsConfigurationRegister() : - rate(protocol::uart::GPSRATE_5HZ), - antPow(protocol::uart::ANTPOWER_OFFRESV) - { } - - /// \brief Creates an initializes a new GpsConfigurationRegister structure (deprecated). - /// - /// \param[in] modeIn Value to initialize the mode field with. - /// \param[in] ppsSourceIn Value to initialize the ppsSource field with. - GpsConfigurationRegister( - protocol::uart::GpsMode modeIn, - protocol::uart::PpsSource ppsSourceIn) : - mode(modeIn), - ppsSource(ppsSourceIn), - rate(protocol::uart::GPSRATE_5HZ), - antPow(protocol::uart::ANTPOWER_OFFRESV) - { } - - /// \brief Creates an initializes a new GpsConfigurationRegister structure. - /// - /// \param[in] modeIn Value to initialize the mode field with. - /// \param[in] ppsSourceIn Value to initialize the ppsSource field with. - /// \param[in] rateIn Value to initialize the rate field with. - /// \param[in] antPowIn Value to initialize the antPow field with. - GpsConfigurationRegister( - protocol::uart::GpsMode modeIn, - protocol::uart::PpsSource ppsSourceIn, - protocol::uart::GpsRate rateIn, - protocol::uart::AntPower antPowIn) : - mode(modeIn), - ppsSource(ppsSourceIn), - rate(rateIn), - antPow(antPowIn) - { } + protocol::uart::GpsMode mode; ///< The mode field. + protocol::uart::PpsSource ppsSource; ///< The ppsSource field. + protocol::uart::GpsRate rate; ///< The rate field. + protocol::uart::AntPower antPow; ///< The antPow field. + + GpsConfigurationRegister() + : rate(protocol::uart::GPSRATE_5HZ), antPow(protocol::uart::ANTPOWER_OFFRESV) + { + } + + /// \brief Creates an initializes a new GpsConfigurationRegister structure (deprecated). + /// + /// \param[in] modeIn Value to initialize the mode field with. + /// \param[in] ppsSourceIn Value to initialize the ppsSource field with. + GpsConfigurationRegister(protocol::uart::GpsMode modeIn, protocol::uart::PpsSource ppsSourceIn) + : mode(modeIn), + ppsSource(ppsSourceIn), + rate(protocol::uart::GPSRATE_5HZ), + antPow(protocol::uart::ANTPOWER_OFFRESV) + { + } + + /// \brief Creates an initializes a new GpsConfigurationRegister structure. + /// + /// \param[in] modeIn Value to initialize the mode field with. + /// \param[in] ppsSourceIn Value to initialize the ppsSource field with. + /// \param[in] rateIn Value to initialize the rate field with. + /// \param[in] antPowIn Value to initialize the antPow field with. + GpsConfigurationRegister( + protocol::uart::GpsMode modeIn, protocol::uart::PpsSource ppsSourceIn, + protocol::uart::GpsRate rateIn, protocol::uart::AntPower antPowIn) + : mode(modeIn), ppsSource(ppsSourceIn), rate(rateIn), antPow(antPowIn) + { + } }; /// \brief Structure representing the GPS Solution - LLA register. struct GpsSolutionLlaRegister { - double time; ///< The time field. - uint16_t week; ///< The week field. - protocol::uart::GpsFix gpsFix; ///< The gpsFix field. - uint8_t numSats; ///< The numSats field. - vn::math::vec3d lla; ///< The lla field. - vn::math::vec3f nedVel; ///< The nedVel field. - vn::math::vec3f nedAcc; ///< The nedAcc field. - float speedAcc; ///< The speedAcc field. - float timeAcc; ///< The timeAcc field. - - GpsSolutionLlaRegister() { } - - /// \brief Creates an initializes a new GpsSolutionLlaRegister structure. - /// - /// \param[in] timeIn Value to initialize the time field with. - /// \param[in] weekIn Value to initialize the week field with. - /// \param[in] gpsFixIn Value to initialize the gpsFix field with. - /// \param[in] numSatsIn Value to initialize the numSats field with. - /// \param[in] llaIn Value to initialize the lla field with. - /// \param[in] nedVelIn Value to initialize the nedVel field with. - /// \param[in] nedAccIn Value to initialize the nedAcc field with. - /// \param[in] speedAccIn Value to initialize the speedAcc field with. - /// \param[in] timeAccIn Value to initialize the timeAcc field with. - GpsSolutionLlaRegister( - double timeIn, - uint16_t weekIn, - protocol::uart::GpsFix gpsFixIn, - uint8_t numSatsIn, - vn::math::vec3d llaIn, - vn::math::vec3f nedVelIn, - vn::math::vec3f nedAccIn, - float speedAccIn, - float timeAccIn) : - time(timeIn), - week(weekIn), - gpsFix(gpsFixIn), - numSats(numSatsIn), - lla(llaIn), - nedVel(nedVelIn), - nedAcc(nedAccIn), - speedAcc(speedAccIn), - timeAcc(timeAccIn) - { } - + double time; ///< The time field. + uint16_t week; ///< The week field. + protocol::uart::GpsFix gpsFix; ///< The gpsFix field. + uint8_t numSats; ///< The numSats field. + vn::math::vec3d lla; ///< The lla field. + vn::math::vec3f nedVel; ///< The nedVel field. + vn::math::vec3f nedAcc; ///< The nedAcc field. + float speedAcc; ///< The speedAcc field. + float timeAcc; ///< The timeAcc field. + + GpsSolutionLlaRegister() {} + + /// \brief Creates an initializes a new GpsSolutionLlaRegister structure. + /// + /// \param[in] timeIn Value to initialize the time field with. + /// \param[in] weekIn Value to initialize the week field with. + /// \param[in] gpsFixIn Value to initialize the gpsFix field with. + /// \param[in] numSatsIn Value to initialize the numSats field with. + /// \param[in] llaIn Value to initialize the lla field with. + /// \param[in] nedVelIn Value to initialize the nedVel field with. + /// \param[in] nedAccIn Value to initialize the nedAcc field with. + /// \param[in] speedAccIn Value to initialize the speedAcc field with. + /// \param[in] timeAccIn Value to initialize the timeAcc field with. + GpsSolutionLlaRegister( + double timeIn, uint16_t weekIn, protocol::uart::GpsFix gpsFixIn, uint8_t numSatsIn, + vn::math::vec3d llaIn, vn::math::vec3f nedVelIn, vn::math::vec3f nedAccIn, float speedAccIn, + float timeAccIn) + : time(timeIn), + week(weekIn), + gpsFix(gpsFixIn), + numSats(numSatsIn), + lla(llaIn), + nedVel(nedVelIn), + nedAcc(nedAccIn), + speedAcc(speedAccIn), + timeAcc(timeAccIn) + { + } }; /// \brief Structure representing the GPS Solution - ECEF register. struct GpsSolutionEcefRegister { - double tow; ///< The tow field. - uint16_t week; ///< The week field. - protocol::uart::GpsFix gpsFix; ///< The gpsFix field. - uint8_t numSats; ///< The numSats field. - vn::math::vec3d position; ///< The position field. - vn::math::vec3f velocity; ///< The velocity field. - vn::math::vec3f posAcc; ///< The posAcc field. - float speedAcc; ///< The speedAcc field. - float timeAcc; ///< The timeAcc field. - - GpsSolutionEcefRegister() { } - - /// \brief Creates an initializes a new GpsSolutionEcefRegister structure. - /// - /// \param[in] towIn Value to initialize the tow field with. - /// \param[in] weekIn Value to initialize the week field with. - /// \param[in] gpsFixIn Value to initialize the gpsFix field with. - /// \param[in] numSatsIn Value to initialize the numSats field with. - /// \param[in] positionIn Value to initialize the position field with. - /// \param[in] velocityIn Value to initialize the velocity field with. - /// \param[in] posAccIn Value to initialize the posAcc field with. - /// \param[in] speedAccIn Value to initialize the speedAcc field with. - /// \param[in] timeAccIn Value to initialize the timeAcc field with. - GpsSolutionEcefRegister( - double towIn, - uint16_t weekIn, - protocol::uart::GpsFix gpsFixIn, - uint8_t numSatsIn, - vn::math::vec3d positionIn, - vn::math::vec3f velocityIn, - vn::math::vec3f posAccIn, - float speedAccIn, - float timeAccIn) : - tow(towIn), - week(weekIn), - gpsFix(gpsFixIn), - numSats(numSatsIn), - position(positionIn), - velocity(velocityIn), - posAcc(posAccIn), - speedAcc(speedAccIn), - timeAcc(timeAccIn) - { } - + double tow; ///< The tow field. + uint16_t week; ///< The week field. + protocol::uart::GpsFix gpsFix; ///< The gpsFix field. + uint8_t numSats; ///< The numSats field. + vn::math::vec3d position; ///< The position field. + vn::math::vec3f velocity; ///< The velocity field. + vn::math::vec3f posAcc; ///< The posAcc field. + float speedAcc; ///< The speedAcc field. + float timeAcc; ///< The timeAcc field. + + GpsSolutionEcefRegister() {} + + /// \brief Creates an initializes a new GpsSolutionEcefRegister structure. + /// + /// \param[in] towIn Value to initialize the tow field with. + /// \param[in] weekIn Value to initialize the week field with. + /// \param[in] gpsFixIn Value to initialize the gpsFix field with. + /// \param[in] numSatsIn Value to initialize the numSats field with. + /// \param[in] positionIn Value to initialize the position field with. + /// \param[in] velocityIn Value to initialize the velocity field with. + /// \param[in] posAccIn Value to initialize the posAcc field with. + /// \param[in] speedAccIn Value to initialize the speedAcc field with. + /// \param[in] timeAccIn Value to initialize the timeAcc field with. + GpsSolutionEcefRegister( + double towIn, uint16_t weekIn, protocol::uart::GpsFix gpsFixIn, uint8_t numSatsIn, + vn::math::vec3d positionIn, vn::math::vec3f velocityIn, vn::math::vec3f posAccIn, + float speedAccIn, float timeAccIn) + : tow(towIn), + week(weekIn), + gpsFix(gpsFixIn), + numSats(numSatsIn), + position(positionIn), + velocity(velocityIn), + posAcc(posAccIn), + speedAcc(speedAccIn), + timeAcc(timeAccIn) + { + } }; /// \brief Structure representing the INS Solution - LLA register. struct InsSolutionLlaRegister { - double time; ///< The time field. - uint16_t week; ///< The week field. - uint16_t status; ///< The status field. - vn::math::vec3f yawPitchRoll; ///< The yawPitchRoll field. - vn::math::vec3d position; ///< The position field. - vn::math::vec3f nedVel; ///< The nedVel field. - float attUncertainty; ///< The attUncertainty field. - float posUncertainty; ///< The posUncertainty field. - float velUncertainty; ///< The velUncertainty field. - - InsSolutionLlaRegister() { } - - /// \brief Creates an initializes a new InsSolutionLlaRegister structure. - /// - /// \param[in] timeIn Value to initialize the time field with. - /// \param[in] weekIn Value to initialize the week field with. - /// \param[in] statusIn Value to initialize the status field with. - /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. - /// \param[in] positionIn Value to initialize the position field with. - /// \param[in] nedVelIn Value to initialize the nedVel field with. - /// \param[in] attUncertaintyIn Value to initialize the attUncertainty field with. - /// \param[in] posUncertaintyIn Value to initialize the posUncertainty field with. - /// \param[in] velUncertaintyIn Value to initialize the velUncertainty field with. - InsSolutionLlaRegister( - double timeIn, - uint16_t weekIn, - uint16_t statusIn, - vn::math::vec3f yawPitchRollIn, - vn::math::vec3d positionIn, - vn::math::vec3f nedVelIn, - float attUncertaintyIn, - float posUncertaintyIn, - float velUncertaintyIn) : - time(timeIn), - week(weekIn), - status(statusIn), - yawPitchRoll(yawPitchRollIn), - position(positionIn), - nedVel(nedVelIn), - attUncertainty(attUncertaintyIn), - posUncertainty(posUncertaintyIn), - velUncertainty(velUncertaintyIn) - { } - + double time; ///< The time field. + uint16_t week; ///< The week field. + uint16_t status; ///< The status field. + vn::math::vec3f yawPitchRoll; ///< The yawPitchRoll field. + vn::math::vec3d position; ///< The position field. + vn::math::vec3f nedVel; ///< The nedVel field. + float attUncertainty; ///< The attUncertainty field. + float posUncertainty; ///< The posUncertainty field. + float velUncertainty; ///< The velUncertainty field. + + InsSolutionLlaRegister() {} + + /// \brief Creates an initializes a new InsSolutionLlaRegister structure. + /// + /// \param[in] timeIn Value to initialize the time field with. + /// \param[in] weekIn Value to initialize the week field with. + /// \param[in] statusIn Value to initialize the status field with. + /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. + /// \param[in] positionIn Value to initialize the position field with. + /// \param[in] nedVelIn Value to initialize the nedVel field with. + /// \param[in] attUncertaintyIn Value to initialize the attUncertainty field with. + /// \param[in] posUncertaintyIn Value to initialize the posUncertainty field with. + /// \param[in] velUncertaintyIn Value to initialize the velUncertainty field with. + InsSolutionLlaRegister( + double timeIn, uint16_t weekIn, uint16_t statusIn, vn::math::vec3f yawPitchRollIn, + vn::math::vec3d positionIn, vn::math::vec3f nedVelIn, float attUncertaintyIn, + float posUncertaintyIn, float velUncertaintyIn) + : time(timeIn), + week(weekIn), + status(statusIn), + yawPitchRoll(yawPitchRollIn), + position(positionIn), + nedVel(nedVelIn), + attUncertainty(attUncertaintyIn), + posUncertainty(posUncertaintyIn), + velUncertainty(velUncertaintyIn) + { + } }; /// \brief Structure representing the INS Solution - ECEF register. struct InsSolutionEcefRegister { - double time; ///< The time field. - uint16_t week; ///< The week field. - uint16_t status; ///< The status field. - vn::math::vec3f yawPitchRoll; ///< The yawPitchRoll field. - vn::math::vec3d position; ///< The position field. - vn::math::vec3f velocity; ///< The velocity field. - float attUncertainty; ///< The attUncertainty field. - float posUncertainty; ///< The posUncertainty field. - float velUncertainty; ///< The velUncertainty field. - - InsSolutionEcefRegister() { } - - /// \brief Creates an initializes a new InsSolutionEcefRegister structure. - /// - /// \param[in] timeIn Value to initialize the time field with. - /// \param[in] weekIn Value to initialize the week field with. - /// \param[in] statusIn Value to initialize the status field with. - /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. - /// \param[in] positionIn Value to initialize the position field with. - /// \param[in] velocityIn Value to initialize the velocity field with. - /// \param[in] attUncertaintyIn Value to initialize the attUncertainty field with. - /// \param[in] posUncertaintyIn Value to initialize the posUncertainty field with. - /// \param[in] velUncertaintyIn Value to initialize the velUncertainty field with. - InsSolutionEcefRegister( - double timeIn, - uint16_t weekIn, - uint16_t statusIn, - vn::math::vec3f yawPitchRollIn, - vn::math::vec3d positionIn, - vn::math::vec3f velocityIn, - float attUncertaintyIn, - float posUncertaintyIn, - float velUncertaintyIn) : - time(timeIn), - week(weekIn), - status(statusIn), - yawPitchRoll(yawPitchRollIn), - position(positionIn), - velocity(velocityIn), - attUncertainty(attUncertaintyIn), - posUncertainty(posUncertaintyIn), - velUncertainty(velUncertaintyIn) - { } - + double time; ///< The time field. + uint16_t week; ///< The week field. + uint16_t status; ///< The status field. + vn::math::vec3f yawPitchRoll; ///< The yawPitchRoll field. + vn::math::vec3d position; ///< The position field. + vn::math::vec3f velocity; ///< The velocity field. + float attUncertainty; ///< The attUncertainty field. + float posUncertainty; ///< The posUncertainty field. + float velUncertainty; ///< The velUncertainty field. + + InsSolutionEcefRegister() {} + + /// \brief Creates an initializes a new InsSolutionEcefRegister structure. + /// + /// \param[in] timeIn Value to initialize the time field with. + /// \param[in] weekIn Value to initialize the week field with. + /// \param[in] statusIn Value to initialize the status field with. + /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. + /// \param[in] positionIn Value to initialize the position field with. + /// \param[in] velocityIn Value to initialize the velocity field with. + /// \param[in] attUncertaintyIn Value to initialize the attUncertainty field with. + /// \param[in] posUncertaintyIn Value to initialize the posUncertainty field with. + /// \param[in] velUncertaintyIn Value to initialize the velUncertainty field with. + InsSolutionEcefRegister( + double timeIn, uint16_t weekIn, uint16_t statusIn, vn::math::vec3f yawPitchRollIn, + vn::math::vec3d positionIn, vn::math::vec3f velocityIn, float attUncertaintyIn, + float posUncertaintyIn, float velUncertaintyIn) + : time(timeIn), + week(weekIn), + status(statusIn), + yawPitchRoll(yawPitchRollIn), + position(positionIn), + velocity(velocityIn), + attUncertainty(attUncertaintyIn), + posUncertainty(posUncertaintyIn), + velUncertainty(velUncertaintyIn) + { + } }; /// \brief Structure representing the INS Basic Configuration register for a VN-200 sensor. struct InsBasicConfigurationRegisterVn200 { - protocol::uart::Scenario scenario; ///< The scenario field. - bool ahrsAiding; ///< The ahrsAiding field. - - InsBasicConfigurationRegisterVn200() { } - - /// \brief Creates an initializes a new InsBasicConfigurationRegisterVn200 structure. - /// - /// \param[in] scenarioIn Value to initialize the scenario field with. - /// \param[in] ahrsAidingIn Value to initialize the ahrsAiding field with. - InsBasicConfigurationRegisterVn200( - protocol::uart::Scenario scenarioIn, - bool ahrsAidingIn) : - scenario(scenarioIn), - ahrsAiding(ahrsAidingIn) - { } - + protocol::uart::Scenario scenario; ///< The scenario field. + bool ahrsAiding; ///< The ahrsAiding field. + + InsBasicConfigurationRegisterVn200() {} + + /// \brief Creates an initializes a new InsBasicConfigurationRegisterVn200 structure. + /// + /// \param[in] scenarioIn Value to initialize the scenario field with. + /// \param[in] ahrsAidingIn Value to initialize the ahrsAiding field with. + InsBasicConfigurationRegisterVn200(protocol::uart::Scenario scenarioIn, bool ahrsAidingIn) + : scenario(scenarioIn), ahrsAiding(ahrsAidingIn) + { + } }; /// \brief Structure representing the INS Basic Configuration register for a VN-300 sensor. struct InsBasicConfigurationRegisterVn300 { - protocol::uart::Scenario scenario; ///< The scenario field. - bool ahrsAiding; ///< The ahrsAiding field. - bool estBaseline; ///< The estBaseline field. - - InsBasicConfigurationRegisterVn300() { } - - /// \brief Creates an initializes a new InsBasicConfigurationRegisterVn300 structure. - /// - /// \param[in] scenarioIn Value to initialize the scenario field with. - /// \param[in] ahrsAidingIn Value to initialize the ahrsAiding field with. - /// \param[in] estBaselineIn Value to initialize the estBaseline field with. - InsBasicConfigurationRegisterVn300( - protocol::uart::Scenario scenarioIn, - bool ahrsAidingIn, - bool estBaselineIn) : - scenario(scenarioIn), - ahrsAiding(ahrsAidingIn), - estBaseline(estBaselineIn) - { } - + protocol::uart::Scenario scenario; ///< The scenario field. + bool ahrsAiding; ///< The ahrsAiding field. + bool estBaseline; ///< The estBaseline field. + + InsBasicConfigurationRegisterVn300() {} + + /// \brief Creates an initializes a new InsBasicConfigurationRegisterVn300 structure. + /// + /// \param[in] scenarioIn Value to initialize the scenario field with. + /// \param[in] ahrsAidingIn Value to initialize the ahrsAiding field with. + /// \param[in] estBaselineIn Value to initialize the estBaseline field with. + InsBasicConfigurationRegisterVn300( + protocol::uart::Scenario scenarioIn, bool ahrsAidingIn, bool estBaselineIn) + : scenario(scenarioIn), ahrsAiding(ahrsAidingIn), estBaseline(estBaselineIn) + { + } }; /// \brief Structure representing the INS Advanced Configuration register. struct InsAdvancedConfigurationRegister { - bool useMag; ///< The useMag field. - bool usePres; ///< The usePres field. - bool posAtt; ///< The posAtt field. - bool velAtt; ///< The velAtt field. - bool velBias; ///< The velBias field. - protocol::uart::FoamInit useFoam; ///< The useFoam field. - uint8_t gpsCovType; ///< The gpsCovType field. - uint8_t velCount; ///< The velCount field. - float velInit; ///< The velInit field. - float moveOrigin; ///< The moveOrigin field. - float gpsTimeout; ///< The gpsTimeout field. - float deltaLimitPos; ///< The deltaLimitPos field. - float deltaLimitVel; ///< The deltaLimitVel field. - float minPosUncertainty; ///< The minPosUncertainty field. - float minVelUncertainty; ///< The minVelUncertainty field. - - InsAdvancedConfigurationRegister() { } - - /// \brief Creates an initializes a new InsAdvancedConfigurationRegister structure. - /// - /// \param[in] useMagIn Value to initialize the useMag field with. - /// \param[in] usePresIn Value to initialize the usePres field with. - /// \param[in] posAttIn Value to initialize the posAtt field with. - /// \param[in] velAttIn Value to initialize the velAtt field with. - /// \param[in] velBiasIn Value to initialize the velBias field with. - /// \param[in] useFoamIn Value to initialize the useFoam field with. - /// \param[in] gpsCovTypeIn Value to initialize the gpsCovType field with. - /// \param[in] velCountIn Value to initialize the velCount field with. - /// \param[in] velInitIn Value to initialize the velInit field with. - /// \param[in] moveOriginIn Value to initialize the moveOrigin field with. - /// \param[in] gpsTimeoutIn Value to initialize the gpsTimeout field with. - /// \param[in] deltaLimitPosIn Value to initialize the deltaLimitPos field with. - /// \param[in] deltaLimitVelIn Value to initialize the deltaLimitVel field with. - /// \param[in] minPosUncertaintyIn Value to initialize the minPosUncertainty field with. - /// \param[in] minVelUncertaintyIn Value to initialize the minVelUncertainty field with. - InsAdvancedConfigurationRegister( - bool useMagIn, - bool usePresIn, - bool posAttIn, - bool velAttIn, - bool velBiasIn, - protocol::uart::FoamInit useFoamIn, - uint8_t gpsCovTypeIn, - uint8_t velCountIn, - float velInitIn, - float moveOriginIn, - float gpsTimeoutIn, - float deltaLimitPosIn, - float deltaLimitVelIn, - float minPosUncertaintyIn, - float minVelUncertaintyIn) : - useMag(useMagIn), - usePres(usePresIn), - posAtt(posAttIn), - velAtt(velAttIn), - velBias(velBiasIn), - useFoam(useFoamIn), - gpsCovType(gpsCovTypeIn), - velCount(velCountIn), - velInit(velInitIn), - moveOrigin(moveOriginIn), - gpsTimeout(gpsTimeoutIn), - deltaLimitPos(deltaLimitPosIn), - deltaLimitVel(deltaLimitVelIn), - minPosUncertainty(minPosUncertaintyIn), - minVelUncertainty(minVelUncertaintyIn) - { } - + bool useMag; ///< The useMag field. + bool usePres; ///< The usePres field. + bool posAtt; ///< The posAtt field. + bool velAtt; ///< The velAtt field. + bool velBias; ///< The velBias field. + protocol::uart::FoamInit useFoam; ///< The useFoam field. + uint8_t gpsCovType; ///< The gpsCovType field. + uint8_t velCount; ///< The velCount field. + float velInit; ///< The velInit field. + float moveOrigin; ///< The moveOrigin field. + float gpsTimeout; ///< The gpsTimeout field. + float deltaLimitPos; ///< The deltaLimitPos field. + float deltaLimitVel; ///< The deltaLimitVel field. + float minPosUncertainty; ///< The minPosUncertainty field. + float minVelUncertainty; ///< The minVelUncertainty field. + + InsAdvancedConfigurationRegister() {} + + /// \brief Creates an initializes a new InsAdvancedConfigurationRegister structure. + /// + /// \param[in] useMagIn Value to initialize the useMag field with. + /// \param[in] usePresIn Value to initialize the usePres field with. + /// \param[in] posAttIn Value to initialize the posAtt field with. + /// \param[in] velAttIn Value to initialize the velAtt field with. + /// \param[in] velBiasIn Value to initialize the velBias field with. + /// \param[in] useFoamIn Value to initialize the useFoam field with. + /// \param[in] gpsCovTypeIn Value to initialize the gpsCovType field with. + /// \param[in] velCountIn Value to initialize the velCount field with. + /// \param[in] velInitIn Value to initialize the velInit field with. + /// \param[in] moveOriginIn Value to initialize the moveOrigin field with. + /// \param[in] gpsTimeoutIn Value to initialize the gpsTimeout field with. + /// \param[in] deltaLimitPosIn Value to initialize the deltaLimitPos field with. + /// \param[in] deltaLimitVelIn Value to initialize the deltaLimitVel field with. + /// \param[in] minPosUncertaintyIn Value to initialize the minPosUncertainty field with. + /// \param[in] minVelUncertaintyIn Value to initialize the minVelUncertainty field with. + InsAdvancedConfigurationRegister( + bool useMagIn, bool usePresIn, bool posAttIn, bool velAttIn, bool velBiasIn, + protocol::uart::FoamInit useFoamIn, uint8_t gpsCovTypeIn, uint8_t velCountIn, float velInitIn, + float moveOriginIn, float gpsTimeoutIn, float deltaLimitPosIn, float deltaLimitVelIn, + float minPosUncertaintyIn, float minVelUncertaintyIn) + : useMag(useMagIn), + usePres(usePresIn), + posAtt(posAttIn), + velAtt(velAttIn), + velBias(velBiasIn), + useFoam(useFoamIn), + gpsCovType(gpsCovTypeIn), + velCount(velCountIn), + velInit(velInitIn), + moveOrigin(moveOriginIn), + gpsTimeout(gpsTimeoutIn), + deltaLimitPos(deltaLimitPosIn), + deltaLimitVel(deltaLimitVelIn), + minPosUncertainty(minPosUncertaintyIn), + minVelUncertainty(minVelUncertaintyIn) + { + } }; /// \brief Structure representing the INS State - LLA register. struct InsStateLlaRegister { - vn::math::vec3f yawPitchRoll; ///< The yawPitchRoll field. - vn::math::vec3d position; ///< The position field. - vn::math::vec3f velocity; ///< The velocity field. - vn::math::vec3f accel; ///< The accel field. - vn::math::vec3f angularRate; ///< The angularRate field. - - InsStateLlaRegister() { } - - /// \brief Creates an initializes a new InsStateLlaRegister structure. - /// - /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. - /// \param[in] positionIn Value to initialize the position field with. - /// \param[in] velocityIn Value to initialize the velocity field with. - /// \param[in] accelIn Value to initialize the accel field with. - /// \param[in] angularRateIn Value to initialize the angularRate field with. - InsStateLlaRegister( - vn::math::vec3f yawPitchRollIn, - vn::math::vec3d positionIn, - vn::math::vec3f velocityIn, - vn::math::vec3f accelIn, - vn::math::vec3f angularRateIn) : - yawPitchRoll(yawPitchRollIn), - position(positionIn), - velocity(velocityIn), - accel(accelIn), - angularRate(angularRateIn) - { } - + vn::math::vec3f yawPitchRoll; ///< The yawPitchRoll field. + vn::math::vec3d position; ///< The position field. + vn::math::vec3f velocity; ///< The velocity field. + vn::math::vec3f accel; ///< The accel field. + vn::math::vec3f angularRate; ///< The angularRate field. + + InsStateLlaRegister() {} + + /// \brief Creates an initializes a new InsStateLlaRegister structure. + /// + /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. + /// \param[in] positionIn Value to initialize the position field with. + /// \param[in] velocityIn Value to initialize the velocity field with. + /// \param[in] accelIn Value to initialize the accel field with. + /// \param[in] angularRateIn Value to initialize the angularRate field with. + InsStateLlaRegister( + vn::math::vec3f yawPitchRollIn, vn::math::vec3d positionIn, vn::math::vec3f velocityIn, + vn::math::vec3f accelIn, vn::math::vec3f angularRateIn) + : yawPitchRoll(yawPitchRollIn), + position(positionIn), + velocity(velocityIn), + accel(accelIn), + angularRate(angularRateIn) + { + } }; /// \brief Structure representing the INS State - ECEF register. struct InsStateEcefRegister { - vn::math::vec3f yawPitchRoll; ///< The yawPitchRoll field. - vn::math::vec3d position; ///< The position field. - vn::math::vec3f velocity; ///< The velocity field. - vn::math::vec3f accel; ///< The accel field. - vn::math::vec3f angularRate; ///< The angularRate field. - - InsStateEcefRegister() { } - - /// \brief Creates an initializes a new InsStateEcefRegister structure. - /// - /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. - /// \param[in] positionIn Value to initialize the position field with. - /// \param[in] velocityIn Value to initialize the velocity field with. - /// \param[in] accelIn Value to initialize the accel field with. - /// \param[in] angularRateIn Value to initialize the angularRate field with. - InsStateEcefRegister( - vn::math::vec3f yawPitchRollIn, - vn::math::vec3d positionIn, - vn::math::vec3f velocityIn, - vn::math::vec3f accelIn, - vn::math::vec3f angularRateIn) : - yawPitchRoll(yawPitchRollIn), - position(positionIn), - velocity(velocityIn), - accel(accelIn), - angularRate(angularRateIn) - { } - + vn::math::vec3f yawPitchRoll; ///< The yawPitchRoll field. + vn::math::vec3d position; ///< The position field. + vn::math::vec3f velocity; ///< The velocity field. + vn::math::vec3f accel; ///< The accel field. + vn::math::vec3f angularRate; ///< The angularRate field. + + InsStateEcefRegister() {} + + /// \brief Creates an initializes a new InsStateEcefRegister structure. + /// + /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. + /// \param[in] positionIn Value to initialize the position field with. + /// \param[in] velocityIn Value to initialize the velocity field with. + /// \param[in] accelIn Value to initialize the accel field with. + /// \param[in] angularRateIn Value to initialize the angularRate field with. + InsStateEcefRegister( + vn::math::vec3f yawPitchRollIn, vn::math::vec3d positionIn, vn::math::vec3f velocityIn, + vn::math::vec3f accelIn, vn::math::vec3f angularRateIn) + : yawPitchRoll(yawPitchRollIn), + position(positionIn), + velocity(velocityIn), + accel(accelIn), + angularRate(angularRateIn) + { + } }; /// \brief Structure representing the Startup Filter Bias Estimate register. struct StartupFilterBiasEstimateRegister { - vn::math::vec3f gyroBias; ///< The gyroBias field. - vn::math::vec3f accelBias; ///< The accelBias field. - float pressureBias; ///< The pressureBias field. - - StartupFilterBiasEstimateRegister() { } - - /// \brief Creates an initializes a new StartupFilterBiasEstimateRegister structure. - /// - /// \param[in] gyroBiasIn Value to initialize the gyroBias field with. - /// \param[in] accelBiasIn Value to initialize the accelBias field with. - /// \param[in] pressureBiasIn Value to initialize the pressureBias field with. - StartupFilterBiasEstimateRegister( - vn::math::vec3f gyroBiasIn, - vn::math::vec3f accelBiasIn, - float pressureBiasIn) : - gyroBias(gyroBiasIn), - accelBias(accelBiasIn), - pressureBias(pressureBiasIn) - { } - + vn::math::vec3f gyroBias; ///< The gyroBias field. + vn::math::vec3f accelBias; ///< The accelBias field. + float pressureBias; ///< The pressureBias field. + + StartupFilterBiasEstimateRegister() {} + + /// \brief Creates an initializes a new StartupFilterBiasEstimateRegister structure. + /// + /// \param[in] gyroBiasIn Value to initialize the gyroBias field with. + /// \param[in] accelBiasIn Value to initialize the accelBias field with. + /// \param[in] pressureBiasIn Value to initialize the pressureBias field with. + StartupFilterBiasEstimateRegister( + vn::math::vec3f gyroBiasIn, vn::math::vec3f accelBiasIn, float pressureBiasIn) + : gyroBias(gyroBiasIn), accelBias(accelBiasIn), pressureBias(pressureBiasIn) + { + } }; /// \brief Structure representing the Delta Theta and Delta Velocity register. struct DeltaThetaAndDeltaVelocityRegister { - float deltaTime; ///< The deltaTime field. - vn::math::vec3f deltaTheta; ///< The deltaTheta field. - vn::math::vec3f deltaVelocity; ///< The deltaVelocity field. - - DeltaThetaAndDeltaVelocityRegister() { } - - /// \brief Creates an initializes a new DeltaThetaAndDeltaVelocityRegister structure. - /// - /// \param[in] deltaTimeIn Value to initialize the deltaTime field with. - /// \param[in] deltaThetaIn Value to initialize the deltaTheta field with. - /// \param[in] deltaVelocityIn Value to initialize the deltaVelocity field with. - DeltaThetaAndDeltaVelocityRegister( - float deltaTimeIn, - vn::math::vec3f deltaThetaIn, - vn::math::vec3f deltaVelocityIn) : - deltaTime(deltaTimeIn), - deltaTheta(deltaThetaIn), - deltaVelocity(deltaVelocityIn) - { } - + float deltaTime; ///< The deltaTime field. + vn::math::vec3f deltaTheta; ///< The deltaTheta field. + vn::math::vec3f deltaVelocity; ///< The deltaVelocity field. + + DeltaThetaAndDeltaVelocityRegister() {} + + /// \brief Creates an initializes a new DeltaThetaAndDeltaVelocityRegister structure. + /// + /// \param[in] deltaTimeIn Value to initialize the deltaTime field with. + /// \param[in] deltaThetaIn Value to initialize the deltaTheta field with. + /// \param[in] deltaVelocityIn Value to initialize the deltaVelocity field with. + DeltaThetaAndDeltaVelocityRegister( + float deltaTimeIn, vn::math::vec3f deltaThetaIn, vn::math::vec3f deltaVelocityIn) + : deltaTime(deltaTimeIn), deltaTheta(deltaThetaIn), deltaVelocity(deltaVelocityIn) + { + } }; /// \brief Structure representing the Delta Theta and Delta Velocity Configuration register. struct DeltaThetaAndDeltaVelocityConfigurationRegister { - protocol::uart::IntegrationFrame integrationFrame; ///< The integrationFrame field. - protocol::uart::CompensationMode gyroCompensation; ///< The gyroCompensation field. - protocol::uart::AccCompensationMode accelCompensation; ///< The accelCompensation field. - protocol::uart::EarthRateCorrection earthRateCorrection; ///< The earthRateCorrection field. - - DeltaThetaAndDeltaVelocityConfigurationRegister() : - earthRateCorrection(protocol::uart::EARTHRATECORR_NONE) - { } - - /// \brief Creates an initializes a new DeltaThetaAndDeltaVelocityConfigurationRegister structure (deprecated). - /// - /// \param[in] integrationFrameIn Value to initialize the integrationFrame field with. - /// \param[in] gyroCompensationIn Value to initialize the gyroCompensation field with. - /// \param[in] accelCompensationIn Value to initialize the accelCompensation field with. - DeltaThetaAndDeltaVelocityConfigurationRegister( - protocol::uart::IntegrationFrame integrationFrameIn, - protocol::uart::CompensationMode gyroCompensationIn, - protocol::uart::AccCompensationMode accelCompensationIn) : - integrationFrame(integrationFrameIn), - gyroCompensation(gyroCompensationIn), - accelCompensation(accelCompensationIn), - earthRateCorrection(protocol::uart::EARTHRATECORR_NONE) - { } - - /// \brief Creates an initializes a new DeltaThetaAndDeltaVelocityConfigurationRegister structure. - /// - /// \param[in] integrationFrameIn Value to initialize the integrationFrame field with. - /// \param[in] gyroCompensationIn Value to initialize the gyroCompensation field with. - /// \param[in] accelCompensationIn Value to initialize the accelCompensation field with. - /// \param[in] earthRateCorrection Value to initialize the earthRateCorrection field with. - DeltaThetaAndDeltaVelocityConfigurationRegister( - protocol::uart::IntegrationFrame integrationFrameIn, - protocol::uart::CompensationMode gyroCompensationIn, - protocol::uart::AccCompensationMode accelCompensationIn, - protocol::uart::EarthRateCorrection earthRateCorrectionIn) : - integrationFrame(integrationFrameIn), - gyroCompensation(gyroCompensationIn), - accelCompensation(accelCompensationIn), - earthRateCorrection(earthRateCorrectionIn) - { } - + protocol::uart::IntegrationFrame integrationFrame; ///< The integrationFrame field. + protocol::uart::CompensationMode gyroCompensation; ///< The gyroCompensation field. + protocol::uart::AccCompensationMode accelCompensation; ///< The accelCompensation field. + protocol::uart::EarthRateCorrection earthRateCorrection; ///< The earthRateCorrection field. + + DeltaThetaAndDeltaVelocityConfigurationRegister() + : earthRateCorrection(protocol::uart::EARTHRATECORR_NONE) + { + } + + /// \brief Creates an initializes a new DeltaThetaAndDeltaVelocityConfigurationRegister structure (deprecated). + /// + /// \param[in] integrationFrameIn Value to initialize the integrationFrame field with. + /// \param[in] gyroCompensationIn Value to initialize the gyroCompensation field with. + /// \param[in] accelCompensationIn Value to initialize the accelCompensation field with. + DeltaThetaAndDeltaVelocityConfigurationRegister( + protocol::uart::IntegrationFrame integrationFrameIn, + protocol::uart::CompensationMode gyroCompensationIn, + protocol::uart::AccCompensationMode accelCompensationIn) + : integrationFrame(integrationFrameIn), + gyroCompensation(gyroCompensationIn), + accelCompensation(accelCompensationIn), + earthRateCorrection(protocol::uart::EARTHRATECORR_NONE) + { + } + + /// \brief Creates an initializes a new DeltaThetaAndDeltaVelocityConfigurationRegister structure. + /// + /// \param[in] integrationFrameIn Value to initialize the integrationFrame field with. + /// \param[in] gyroCompensationIn Value to initialize the gyroCompensation field with. + /// \param[in] accelCompensationIn Value to initialize the accelCompensation field with. + /// \param[in] earthRateCorrection Value to initialize the earthRateCorrection field with. + DeltaThetaAndDeltaVelocityConfigurationRegister( + protocol::uart::IntegrationFrame integrationFrameIn, + protocol::uart::CompensationMode gyroCompensationIn, + protocol::uart::AccCompensationMode accelCompensationIn, + protocol::uart::EarthRateCorrection earthRateCorrectionIn) + : integrationFrame(integrationFrameIn), + gyroCompensation(gyroCompensationIn), + accelCompensation(accelCompensationIn), + earthRateCorrection(earthRateCorrectionIn) + { + } }; /// \brief Structure representing the Reference Vector Configuration register. struct ReferenceVectorConfigurationRegister { - bool useMagModel; ///< The useMagModel field. - bool useGravityModel; ///< The useGravityModel field. - uint32_t recalcThreshold; ///< The recalcThreshold field. - float year; ///< The year field. - vn::math::vec3d position; ///< The position field. - - ReferenceVectorConfigurationRegister() { } - - /// \brief Creates an initializes a new ReferenceVectorConfigurationRegister structure. - /// - /// \param[in] useMagModelIn Value to initialize the useMagModel field with. - /// \param[in] useGravityModelIn Value to initialize the useGravityModel field with. - /// \param[in] recalcThresholdIn Value to initialize the recalcThreshold field with. - /// \param[in] yearIn Value to initialize the year field with. - /// \param[in] positionIn Value to initialize the position field with. - ReferenceVectorConfigurationRegister( - bool useMagModelIn, - bool useGravityModelIn, - uint32_t recalcThresholdIn, - float yearIn, - vn::math::vec3d positionIn) : - useMagModel(useMagModelIn), - useGravityModel(useGravityModelIn), - recalcThreshold(recalcThresholdIn), - year(yearIn), - position(positionIn) - { } - + bool useMagModel; ///< The useMagModel field. + bool useGravityModel; ///< The useGravityModel field. + uint32_t recalcThreshold; ///< The recalcThreshold field. + float year; ///< The year field. + vn::math::vec3d position; ///< The position field. + + ReferenceVectorConfigurationRegister() {} + + /// \brief Creates an initializes a new ReferenceVectorConfigurationRegister structure. + /// + /// \param[in] useMagModelIn Value to initialize the useMagModel field with. + /// \param[in] useGravityModelIn Value to initialize the useGravityModel field with. + /// \param[in] recalcThresholdIn Value to initialize the recalcThreshold field with. + /// \param[in] yearIn Value to initialize the year field with. + /// \param[in] positionIn Value to initialize the position field with. + ReferenceVectorConfigurationRegister( + bool useMagModelIn, bool useGravityModelIn, uint32_t recalcThresholdIn, float yearIn, + vn::math::vec3d positionIn) + : useMagModel(useMagModelIn), + useGravityModel(useGravityModelIn), + recalcThreshold(recalcThresholdIn), + year(yearIn), + position(positionIn) + { + } }; /// \brief Structure representing the Gyro Compensation register. struct GyroCompensationRegister { - vn::math::mat3f c; ///< The c field. - vn::math::vec3f b; ///< The b field. - - GyroCompensationRegister() { } - - /// \brief Creates an initializes a new GyroCompensationRegister structure. - /// - /// \param[in] cIn Value to initialize the c field with. - /// \param[in] bIn Value to initialize the b field with. - GyroCompensationRegister( - vn::math::mat3f cIn, - vn::math::vec3f bIn) : - c(cIn), - b(bIn) - { } + vn::math::mat3f c; ///< The c field. + vn::math::vec3f b; ///< The b field. + + GyroCompensationRegister() {} + /// \brief Creates an initializes a new GyroCompensationRegister structure. + /// + /// \param[in] cIn Value to initialize the c field with. + /// \param[in] bIn Value to initialize the b field with. + GyroCompensationRegister(vn::math::mat3f cIn, vn::math::vec3f bIn) : c(cIn), b(bIn) {} }; /// \brief Structure representing the IMU Filtering Configuration register. struct ImuFilteringConfigurationRegister { - uint16_t magWindowSize; ///< The magWindowSize field. - uint16_t accelWindowSize; ///< The accelWindowSize field. - uint16_t gyroWindowSize; ///< The gyroWindowSize field. - uint16_t tempWindowSize; ///< The tempWindowSize field. - uint16_t presWindowSize; ///< The presWindowSize field. - protocol::uart::FilterMode magFilterMode; ///< The magFilterMode field. - protocol::uart::FilterMode accelFilterMode; ///< The accelFilterMode field. - protocol::uart::FilterMode gyroFilterMode; ///< The gyroFilterMode field. - protocol::uart::FilterMode tempFilterMode; ///< The tempFilterMode field. - protocol::uart::FilterMode presFilterMode; ///< The presFilterMode field. - - ImuFilteringConfigurationRegister() { } - - /// \brief Creates an initializes a new ImuFilteringConfigurationRegister structure. - /// - /// \param[in] magWindowSizeIn Value to initialize the magWindowSize field with. - /// \param[in] accelWindowSizeIn Value to initialize the accelWindowSize field with. - /// \param[in] gyroWindowSizeIn Value to initialize the gyroWindowSize field with. - /// \param[in] tempWindowSizeIn Value to initialize the tempWindowSize field with. - /// \param[in] presWindowSizeIn Value to initialize the presWindowSize field with. - /// \param[in] magFilterModeIn Value to initialize the magFilterMode field with. - /// \param[in] accelFilterModeIn Value to initialize the accelFilterMode field with. - /// \param[in] gyroFilterModeIn Value to initialize the gyroFilterMode field with. - /// \param[in] tempFilterModeIn Value to initialize the tempFilterMode field with. - /// \param[in] presFilterModeIn Value to initialize the presFilterMode field with. - ImuFilteringConfigurationRegister( - uint16_t magWindowSizeIn, - uint16_t accelWindowSizeIn, - uint16_t gyroWindowSizeIn, - uint16_t tempWindowSizeIn, - uint16_t presWindowSizeIn, - protocol::uart::FilterMode magFilterModeIn, - protocol::uart::FilterMode accelFilterModeIn, - protocol::uart::FilterMode gyroFilterModeIn, - protocol::uart::FilterMode tempFilterModeIn, - protocol::uart::FilterMode presFilterModeIn) : - magWindowSize(magWindowSizeIn), - accelWindowSize(accelWindowSizeIn), - gyroWindowSize(gyroWindowSizeIn), - tempWindowSize(tempWindowSizeIn), - presWindowSize(presWindowSizeIn), - magFilterMode(magFilterModeIn), - accelFilterMode(accelFilterModeIn), - gyroFilterMode(gyroFilterModeIn), - tempFilterMode(tempFilterModeIn), - presFilterMode(presFilterModeIn) - { } - + uint16_t magWindowSize; ///< The magWindowSize field. + uint16_t accelWindowSize; ///< The accelWindowSize field. + uint16_t gyroWindowSize; ///< The gyroWindowSize field. + uint16_t tempWindowSize; ///< The tempWindowSize field. + uint16_t presWindowSize; ///< The presWindowSize field. + protocol::uart::FilterMode magFilterMode; ///< The magFilterMode field. + protocol::uart::FilterMode accelFilterMode; ///< The accelFilterMode field. + protocol::uart::FilterMode gyroFilterMode; ///< The gyroFilterMode field. + protocol::uart::FilterMode tempFilterMode; ///< The tempFilterMode field. + protocol::uart::FilterMode presFilterMode; ///< The presFilterMode field. + + ImuFilteringConfigurationRegister() {} + + /// \brief Creates an initializes a new ImuFilteringConfigurationRegister structure. + /// + /// \param[in] magWindowSizeIn Value to initialize the magWindowSize field with. + /// \param[in] accelWindowSizeIn Value to initialize the accelWindowSize field with. + /// \param[in] gyroWindowSizeIn Value to initialize the gyroWindowSize field with. + /// \param[in] tempWindowSizeIn Value to initialize the tempWindowSize field with. + /// \param[in] presWindowSizeIn Value to initialize the presWindowSize field with. + /// \param[in] magFilterModeIn Value to initialize the magFilterMode field with. + /// \param[in] accelFilterModeIn Value to initialize the accelFilterMode field with. + /// \param[in] gyroFilterModeIn Value to initialize the gyroFilterMode field with. + /// \param[in] tempFilterModeIn Value to initialize the tempFilterMode field with. + /// \param[in] presFilterModeIn Value to initialize the presFilterMode field with. + ImuFilteringConfigurationRegister( + uint16_t magWindowSizeIn, uint16_t accelWindowSizeIn, uint16_t gyroWindowSizeIn, + uint16_t tempWindowSizeIn, uint16_t presWindowSizeIn, + protocol::uart::FilterMode magFilterModeIn, protocol::uart::FilterMode accelFilterModeIn, + protocol::uart::FilterMode gyroFilterModeIn, protocol::uart::FilterMode tempFilterModeIn, + protocol::uart::FilterMode presFilterModeIn) + : magWindowSize(magWindowSizeIn), + accelWindowSize(accelWindowSizeIn), + gyroWindowSize(gyroWindowSizeIn), + tempWindowSize(tempWindowSizeIn), + presWindowSize(presWindowSizeIn), + magFilterMode(magFilterModeIn), + accelFilterMode(accelFilterModeIn), + gyroFilterMode(gyroFilterModeIn), + tempFilterMode(tempFilterModeIn), + presFilterMode(presFilterModeIn) + { + } }; /// \brief Structure representing the GPS Compass Baseline register. struct GpsCompassBaselineRegister { - vn::math::vec3f position; ///< The position field. - vn::math::vec3f uncertainty; ///< The uncertainty field. - - GpsCompassBaselineRegister() { } - - /// \brief Creates an initializes a new GpsCompassBaselineRegister structure. - /// - /// \param[in] positionIn Value to initialize the position field with. - /// \param[in] uncertaintyIn Value to initialize the uncertainty field with. - GpsCompassBaselineRegister( - vn::math::vec3f positionIn, - vn::math::vec3f uncertaintyIn) : - position(positionIn), - uncertainty(uncertaintyIn) - { } - + vn::math::vec3f position; ///< The position field. + vn::math::vec3f uncertainty; ///< The uncertainty field. + + GpsCompassBaselineRegister() {} + + /// \brief Creates an initializes a new GpsCompassBaselineRegister structure. + /// + /// \param[in] positionIn Value to initialize the position field with. + /// \param[in] uncertaintyIn Value to initialize the uncertainty field with. + GpsCompassBaselineRegister(vn::math::vec3f positionIn, vn::math::vec3f uncertaintyIn) + : position(positionIn), uncertainty(uncertaintyIn) + { + } }; /// \brief Structure representing the GPS Compass Estimated Baseline register. struct GpsCompassEstimatedBaselineRegister { - bool estBaselineUsed; ///< The estBaselineUsed field. - uint16_t numMeas; ///< The numMeas field. - vn::math::vec3f position; ///< The position field. - vn::math::vec3f uncertainty; ///< The uncertainty field. - - GpsCompassEstimatedBaselineRegister() { } - - /// \brief Creates an initializes a new GpsCompassEstimatedBaselineRegister structure. - /// - /// \param[in] estBaselineUsedIn Value to initialize the estBaselineUsed field with. - /// \param[in] numMeasIn Value to initialize the numMeas field with. - /// \param[in] positionIn Value to initialize the position field with. - /// \param[in] uncertaintyIn Value to initialize the uncertainty field with. - GpsCompassEstimatedBaselineRegister( - bool estBaselineUsedIn, - uint16_t numMeasIn, - vn::math::vec3f positionIn, - vn::math::vec3f uncertaintyIn) : - estBaselineUsed(estBaselineUsedIn), - numMeas(numMeasIn), - position(positionIn), - uncertainty(uncertaintyIn) - { } - + bool estBaselineUsed; ///< The estBaselineUsed field. + uint16_t numMeas; ///< The numMeas field. + vn::math::vec3f position; ///< The position field. + vn::math::vec3f uncertainty; ///< The uncertainty field. + + GpsCompassEstimatedBaselineRegister() {} + + /// \brief Creates an initializes a new GpsCompassEstimatedBaselineRegister structure. + /// + /// \param[in] estBaselineUsedIn Value to initialize the estBaselineUsed field with. + /// \param[in] numMeasIn Value to initialize the numMeas field with. + /// \param[in] positionIn Value to initialize the position field with. + /// \param[in] uncertaintyIn Value to initialize the uncertainty field with. + GpsCompassEstimatedBaselineRegister( + bool estBaselineUsedIn, uint16_t numMeasIn, vn::math::vec3f positionIn, + vn::math::vec3f uncertaintyIn) + : estBaselineUsed(estBaselineUsedIn), + numMeas(numMeasIn), + position(positionIn), + uncertainty(uncertaintyIn) + { + } }; /// \brief Structure representing the IMU Rate Configuration register. struct ImuRateConfigurationRegister { - uint16_t imuRate; ///< The imuRate field. - uint16_t navDivisor; ///< The navDivisor field. - float filterTargetRate; ///< The filterTargetRate field. - float filterMinRate; ///< The filterMinRate field. - - ImuRateConfigurationRegister() { } - - /// \brief Creates an initializes a new ImuRateConfigurationRegister structure. - /// - /// \param[in] imuRateIn Value to initialize the imuRate field with. - /// \param[in] navDivisorIn Value to initialize the navDivisor field with. - /// \param[in] filterTargetRateIn Value to initialize the filterTargetRate field with. - /// \param[in] filterMinRateIn Value to initialize the filterMinRate field with. - ImuRateConfigurationRegister( - uint16_t imuRateIn, - uint16_t navDivisorIn, - float filterTargetRateIn, - float filterMinRateIn) : - imuRate(imuRateIn), - navDivisor(navDivisorIn), - filterTargetRate(filterTargetRateIn), - filterMinRate(filterMinRateIn) - { } - + uint16_t imuRate; ///< The imuRate field. + uint16_t navDivisor; ///< The navDivisor field. + float filterTargetRate; ///< The filterTargetRate field. + float filterMinRate; ///< The filterMinRate field. + + ImuRateConfigurationRegister() {} + + /// \brief Creates an initializes a new ImuRateConfigurationRegister structure. + /// + /// \param[in] imuRateIn Value to initialize the imuRate field with. + /// \param[in] navDivisorIn Value to initialize the navDivisor field with. + /// \param[in] filterTargetRateIn Value to initialize the filterTargetRate field with. + /// \param[in] filterMinRateIn Value to initialize the filterMinRate field with. + ImuRateConfigurationRegister( + uint16_t imuRateIn, uint16_t navDivisorIn, float filterTargetRateIn, float filterMinRateIn) + : imuRate(imuRateIn), + navDivisor(navDivisorIn), + filterTargetRate(filterTargetRateIn), + filterMinRate(filterMinRateIn) + { + } }; /// \brief Structure representing the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register. struct YawPitchRollTrueBodyAccelerationAndAngularRatesRegister { - vn::math::vec3f yawPitchRoll; ///< The yawPitchRoll field. - vn::math::vec3f bodyAccel; ///< The bodyAccel field. - vn::math::vec3f gyro; ///< The gyro field. - - YawPitchRollTrueBodyAccelerationAndAngularRatesRegister() { } - - /// \brief Creates an initializes a new YawPitchRollTrueBodyAccelerationAndAngularRatesRegister structure. - /// - /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. - /// \param[in] bodyAccelIn Value to initialize the bodyAccel field with. - /// \param[in] gyroIn Value to initialize the gyro field with. - YawPitchRollTrueBodyAccelerationAndAngularRatesRegister( - vn::math::vec3f yawPitchRollIn, - vn::math::vec3f bodyAccelIn, - vn::math::vec3f gyroIn) : - yawPitchRoll(yawPitchRollIn), - bodyAccel(bodyAccelIn), - gyro(gyroIn) - { } - + vn::math::vec3f yawPitchRoll; ///< The yawPitchRoll field. + vn::math::vec3f bodyAccel; ///< The bodyAccel field. + vn::math::vec3f gyro; ///< The gyro field. + + YawPitchRollTrueBodyAccelerationAndAngularRatesRegister() {} + + /// \brief Creates an initializes a new YawPitchRollTrueBodyAccelerationAndAngularRatesRegister structure. + /// + /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. + /// \param[in] bodyAccelIn Value to initialize the bodyAccel field with. + /// \param[in] gyroIn Value to initialize the gyro field with. + YawPitchRollTrueBodyAccelerationAndAngularRatesRegister( + vn::math::vec3f yawPitchRollIn, vn::math::vec3f bodyAccelIn, vn::math::vec3f gyroIn) + : yawPitchRoll(yawPitchRollIn), bodyAccel(bodyAccelIn), gyro(gyroIn) + { + } }; /// \brief Structure representing the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register. struct YawPitchRollTrueInertialAccelerationAndAngularRatesRegister { - vn::math::vec3f yawPitchRoll; ///< The yawPitchRoll field. - vn::math::vec3f inertialAccel; ///< The inertialAccel field. - vn::math::vec3f gyro; ///< The gyro field. - - YawPitchRollTrueInertialAccelerationAndAngularRatesRegister() { } - - /// \brief Creates an initializes a new YawPitchRollTrueInertialAccelerationAndAngularRatesRegister structure. - /// - /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. - /// \param[in] inertialAccelIn Value to initialize the inertialAccel field with. - /// \param[in] gyroIn Value to initialize the gyro field with. - YawPitchRollTrueInertialAccelerationAndAngularRatesRegister( - vn::math::vec3f yawPitchRollIn, - vn::math::vec3f inertialAccelIn, - vn::math::vec3f gyroIn) : - yawPitchRoll(yawPitchRollIn), - inertialAccel(inertialAccelIn), - gyro(gyroIn) - { } - + vn::math::vec3f yawPitchRoll; ///< The yawPitchRoll field. + vn::math::vec3f inertialAccel; ///< The inertialAccel field. + vn::math::vec3f gyro; ///< The gyro field. + + YawPitchRollTrueInertialAccelerationAndAngularRatesRegister() {} + + /// \brief Creates an initializes a new YawPitchRollTrueInertialAccelerationAndAngularRatesRegister structure. + /// + /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. + /// \param[in] inertialAccelIn Value to initialize the inertialAccel field with. + /// \param[in] gyroIn Value to initialize the gyro field with. + YawPitchRollTrueInertialAccelerationAndAngularRatesRegister( + vn::math::vec3f yawPitchRollIn, vn::math::vec3f inertialAccelIn, vn::math::vec3f gyroIn) + : yawPitchRoll(yawPitchRollIn), inertialAccel(inertialAccelIn), gyro(gyroIn) + { + } }; /// \} -} -} +} // namespace sensors +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/rtcmlistener.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/rtcmlistener.h index 67b9d6dc..88410334 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/rtcmlistener.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/rtcmlistener.h @@ -1,101 +1,99 @@ #ifndef _VN_XPLAT_RTCMLISTENER_H_ #define _VN_XPLAT_RTCMLISTENER_H_ -#include "rtcmmessage.h" -#include "export.h" - -#include // Used for the call back function #include +#include // Used for the call back function + +#include "export.h" +#include "rtcmmessage.h" + #ifdef _WIN32 #ifndef _WIN32_WINNT - #define _WIN32_WINNT 0x0501 +#define _WIN32_WINNT 0x0501 #endif #include #include -#else -#include // Use POSIX-style sockets +#else #include -#include // Needed for getaddrinfo() and freeaddrinfo() -#include // Needed for close() +#include // Needed for getaddrinfo() and freeaddrinfo() +#include // Use POSIX-style sockets +#include // Needed for close() //#include #endif #include "thread.h" -namespace vn { - namespace xplat { - - /// \brief Helpful class for working with RTCM broadcasters. - class vn_proglib_DLLEXPORT rtcmlistener - { - public: - rtcmlistener(); - - /// \brief Connect to the broadcasting server - /// - /// \return bool True if successful, otherwise false. - bool connectToServer(); - - /// \brief Disconnect from the broadcasting server - /// - /// \return bool True if successful, otherwise false. - bool disconnectFromServer(); - - - /// \brief Register a callback funtion to be notified when a message arrives - /// - /// \param[in] notificationHandler The callback function to register - void registerNotifications(std::function notificationHandler); - - /// \brief The host address of the broadcasting server - /// - /// \param[in] host the IP of the host - void setServerHost(std::string host); - - /// \brief Set the server port of the boardcasting server - /// - /// \param[in] port the port to assign - void setServerPort(std::string port); - - /// \brief Thread to processing incoming messages from a RTCM broadcast server (used by the class when connecting to the server) - void communicationThreadWithRTCMServer(); - - - - - private: +namespace vn +{ +namespace xplat +{ + +/// \brief Helpful class for working with RTCM broadcasters. +class vn_proglib_DLLEXPORT rtcmlistener +{ +public: + rtcmlistener(); + + /// \brief Connect to the broadcasting server + /// + /// \return bool True if successful, otherwise false. + bool connectToServer(); + + /// \brief Disconnect from the broadcasting server + /// + /// \return bool True if successful, otherwise false. + bool disconnectFromServer(); + + /// \brief Register a callback funtion to be notified when a message arrives + /// + /// \param[in] notificationHandler The callback function to register + void registerNotifications(std::function notificationHandler); + + /// \brief The host address of the broadcasting server + /// + /// \param[in] host the IP of the host + void setServerHost(std::string host); + + /// \brief Set the server port of the boardcasting server + /// + /// \param[in] port the port to assign + void setServerPort(std::string port); + + /// \brief Thread to processing incoming messages from a RTCM broadcast server (used by the class when connecting to the server) + void communicationThreadWithRTCMServer(); + +private: #ifdef _WIN32 - #define GETSOCKETERROR() (WSAGetLastError()) +#define GETSOCKETERROR() (WSAGetLastError()) #else - #define GETSOCKETERROR() (errno) - #define NO_ERROR 0 - #define INVALID_SOCKET -1 - #define SOCKET_ERROR -1 - typedef int SOCKET; +#define GETSOCKETERROR() (errno) +#define NO_ERROR 0 +#define INVALID_SOCKET -1 +#define SOCKET_ERROR -1 + typedef int SOCKET; #endif - int sockInit(); - int sockQuit(); - int sockClose(SOCKET s); - bool sockValid(SOCKET s); - void printBuffer(char* buffer, int bufferSize); - - bool timeToQuit = false; - std::string serverHost = "localhost"; - std::string serverPort = "8678"; + int sockInit(); + int sockQuit(); + int sockClose(SOCKET s); + bool sockValid(SOCKET s); + void printBuffer(char * buffer, int bufferSize); - // POSIX-style sockets are int with negative values being invalid, while WinSock sockets are unsigned int with a special INVALID_SOCKET instead. - SOCKET serverSocket = INVALID_SOCKET; + bool timeToQuit = false; + std::string serverHost = "localhost"; + std::string serverPort = "8678"; - //std::unique_ptr serverThread = nullptr; - Thread* serverThread = nullptr; - std::function messageNotification; - bool messageNotificationActive = false; + // POSIX-style sockets are int with negative values being invalid, while WinSock sockets are unsigned int with a special INVALID_SOCKET instead. + SOCKET serverSocket = INVALID_SOCKET; - }; + //std::unique_ptr serverThread = nullptr; + Thread * serverThread = nullptr; + std::function messageNotification; + bool messageNotificationActive = false; +}; - } +} // namespace xplat -} +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/rtcmmessage.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/rtcmmessage.h index d1892ae7..274e22eb 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/rtcmmessage.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/rtcmmessage.h @@ -1,82 +1,81 @@ #ifndef _VN_XPLAT_RTCMMESSAGE_H_ #define _VN_XPLAT_RTCMMESSAGE_H_ -#include #include + +#include + #include "export.h" -namespace vn { - namespace xplat { - /// \brief Helpful class for working with RTCM messages. - class vn_proglib_DLLEXPORT rtcmmessage - { - public: - /// \brief A flag to indicate whether or not this RTCM message object is valid - bool valid; - /// \brief A flag to indicate whether or not this RTCM message object is support on VectorNav sensors - bool supported; - /// \brief The RTCM group that the message belongs to - std::string group; - /// \brief The RTCM message ID - int id; - /// \brief The raw buffer for the received message - char* buffer; +namespace vn +{ +namespace xplat +{ +/// \brief Helpful class for working with RTCM messages. +class vn_proglib_DLLEXPORT rtcmmessage +{ +public: + /// \brief A flag to indicate whether or not this RTCM message object is valid + bool valid; + /// \brief A flag to indicate whether or not this RTCM message object is support on VectorNav sensors + bool supported; + /// \brief The RTCM group that the message belongs to + std::string group; + /// \brief The RTCM message ID + int id; + /// \brief The raw buffer for the received message + char * buffer; - /// \brief Creates a RTCM message object - /// - /// \param[in] buffer A buffer to parse. - /// \param[in] bufferSize The buffer size. - /// \param[in] offset The starting offset within the buffer. - /// \param[in] messageSize The size of the message. (messageSize = (((buffer[offset + 1] & 0xFF) & 0x03) * 256) + (buffer[offset + 2] & 0xFF) + 6) - rtcmmessage(char* buffer, int bufferSize, int offset, int messageSize); + /// \brief Creates a RTCM message object + /// + /// \param[in] buffer A buffer to parse. + /// \param[in] bufferSize The buffer size. + /// \param[in] offset The starting offset within the buffer. + /// \param[in] messageSize The size of the message. (messageSize = (((buffer[offset + 1] & 0xFF) & 0x03) * 256) + (buffer[offset + 2] & 0xFF) + 6) + rtcmmessage(char * buffer, int bufferSize, int offset, int messageSize); - /// \brief Unit test for the RTCM message class - /// - /// \return bool True if successful, otherwise false. - static bool unitTest(); - private: - unsigned long crc; - void processBuffer(int messageSize); - void printBuffer(char* buffer, int bufferSize); + /// \brief Unit test for the RTCM message class + /// + /// \return bool True if successful, otherwise false. + static bool unitTest(); - // Lookup Table for calculating the CRC - unsigned int crc24qtab[256] = - { - 0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17, - 0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E, - 0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E, - 0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7, - 0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE, - 0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7, - 0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077, - 0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E, - 0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5, - 0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC, - 0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C, - 0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375, - 0x15723B, 0x933EC0, 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C, - 0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15, - 0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5, - 0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C, - 0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3, - 0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A, - 0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A, - 0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703, - 0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A, - 0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863, - 0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3, - 0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA, - 0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61, - 0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58, - 0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8, - 0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1, - 0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88, - 0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1, - 0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401, - 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538 - }; +private: + unsigned long crc; + void processBuffer(int messageSize); + void printBuffer(char * buffer, int bufferSize); - }; - } -} + // Lookup Table for calculating the CRC + unsigned int crc24qtab[256] = { + 0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17, 0xA18139, + 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E, 0xC54E89, 0x430272, + 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E, 0x64CFB0, 0xE2834B, 0xEE1ABD, + 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7, 0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, + 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE, 0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, + 0xB8FACA, 0xB4633C, 0x322FC7, 0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, + 0xD0AC8C, 0x56E077, 0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, + 0xF7614E, 0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5, + 0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC, 0xDCED5B, + 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C, 0x7D6C62, 0xFB2099, + 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375, 0x15723B, 0x933EC0, 0x9FA736, + 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C, 0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, + 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15, 0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, + 0xC596A8, 0xC90F5E, 0x4F43A5, 0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, + 0x688E67, 0xEEC29C, 0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, + 0xAC38B3, 0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A, + 0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A, 0x578814, + 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703, 0x3F964D, 0xB9DAB6, + 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A, 0x9E1774, 0x185B8F, 0x14C279, + 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863, 0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, + 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3, 0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, + 0x4EF3E7, 0x426A11, 0xC426EA, 0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, + 0x33D79A, 0xB59B61, 0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, + 0x141A58, 0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8, + 0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1, 0x26359F, + 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88, 0x87B4A6, 0x01F85D, + 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1, 0xE37B16, 0x6537ED, 0x69AE1B, + 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401, 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, + 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538}; +}; +} // namespace xplat +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/searcher.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/searcher.h index 277b4ab0..490198cf 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/searcher.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/searcher.h @@ -4,59 +4,60 @@ #include #include -#include "int.h" #include "export.h" +#include "int.h" -namespace vn { -namespace sensors { +namespace vn +{ +namespace sensors +{ /// \brief Helpful class for finding VectorNav sensors. class vn_proglib_DLLEXPORT Searcher { - public: - - /// \brief Searches the serial port at all valid baudrates for a VectorNav - /// sensor. - /// - /// \param[in] portName The serial port to search. - /// \param[out] foundBuadrate If a sensor is found, this will be set to the - /// baudrate the sensor is communicating at. - /// \returns true if a sensor if found; otherwise false. - static void findPorts(std::vector& portlist); - - /// \brief Searches the serial port at all valid baudrates for a VectorNav - /// sensor. - /// - /// \param[in] portName The serial port to search. - /// \param[out] foundBuadrate If a sensor is found, this will be set to the - /// baudrate the sensor is communicating at. - /// \returns true if a sensor if found; otherwise false. - static bool search(const std::string &portName, int32_t *foundBaudrate); - - /// \brief Checks all available serial ports on the system for any - /// VectorNav sensors. - /// - /// \return Collection of serial ports and baudrates for all found sensors. - static std::vector > search(void); - - /// \brief Checks the provided list of serial ports for any connected - /// VectorNav sensors. - /// - /// \param[in] portsToCheck List of serial ports to check for sensors. - /// \return Collection of serial ports and baudrates for all found sensors. - static std::vector > search(std::vector& portsToCheck); - - /// \brief Tests if a sensor is connected to the serial port at the - /// specified baudrate. - /// - /// \param[in] portName The serial port to test. - /// \param[in] baudrate The baudrate to test at. - /// \returns true if a sensor if found; otherwise false. - static bool test(std::string portName, uint32_t baudrate); + /// \brief Searches the serial port at all valid baudrates for a VectorNav + /// sensor. + /// + /// \param[in] portName The serial port to search. + /// \param[out] foundBuadrate If a sensor is found, this will be set to the + /// baudrate the sensor is communicating at. + /// \returns true if a sensor if found; otherwise false. + static void findPorts(std::vector & portlist); + + /// \brief Searches the serial port at all valid baudrates for a VectorNav + /// sensor. + /// + /// \param[in] portName The serial port to search. + /// \param[out] foundBuadrate If a sensor is found, this will be set to the + /// baudrate the sensor is communicating at. + /// \returns true if a sensor if found; otherwise false. + static bool search(const std::string & portName, int32_t * foundBaudrate); + + /// \brief Checks all available serial ports on the system for any + /// VectorNav sensors. + /// + /// \return Collection of serial ports and baudrates for all found sensors. + static std::vector > search(void); + + /// \brief Checks the provided list of serial ports for any connected + /// VectorNav sensors. + /// + /// \param[in] portsToCheck List of serial ports to check for sensors. + /// \return Collection of serial ports and baudrates for all found sensors. + static std::vector > search( + std::vector & portsToCheck); + + /// \brief Tests if a sensor is connected to the serial port at the + /// specified baudrate. + /// + /// \param[in] portName The serial port to test. + /// \param[in] baudrate The baudrate to test at. + /// \returns true if a sensor if found; otherwise false. + static bool test(std::string portName, uint32_t baudrate); }; -} -} +} // namespace sensors +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/sensors.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/sensors.h index b8d5b559..5764dc15 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/sensors.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/sensors.h @@ -2,28 +2,31 @@ #define _VNSENSORS_SENSORS_H_ #if PYTHON - #include "boostpython.h" +#include "boostpython.h" #endif #include #include +#include "export.h" #include "int.h" #include "nocopy.h" #include "packetfinder.h" -#include "export.h" #include "registers.h" #if PYTHON - #include "vn/event.h" +#include "vn/event.h" #endif -namespace vn { +namespace vn +{ -namespace xplat { - class IPort; +namespace xplat +{ +class IPort; } -namespace sensors { +namespace sensors +{ // Define the max record size of a firmware update file #define MAXFIRMWAREUPDATERECORDSIZE 300 @@ -32,1531 +35,1468 @@ namespace sensors { struct sensor_error : public std::exception { private: - sensor_error(); + sensor_error(); public: + /// \brief Creates a new sensor_error based on the error value provided by + /// the sensor. + explicit sensor_error(protocol::uart::SensorError e); - /// \brief Creates a new sensor_error based on the error value provided by - /// the sensor. - explicit sensor_error(protocol::uart::SensorError e); - - /// \brief Copy constructor. - sensor_error(const sensor_error& e); + /// \brief Copy constructor. + sensor_error(const sensor_error & e); - ~sensor_error() throw(); + ~sensor_error() throw(); - /// \brief Returns a description of the exception. - /// - /// \return A description of the exception. - char const* what() const throw(); + /// \brief Returns a description of the exception. + /// + /// \return A description of the exception. + char const * what() const throw(); - /// \brief The associated sensor error. - protocol::uart::SensorError error; + /// \brief The associated sensor error. + protocol::uart::SensorError error; private: - char *_errorMessage; + char * _errorMessage; }; /// \brief Helpful class for working with VectorNav sensors. class vn_proglib_DLLEXPORT VnSensor : private util::NoCopy { - public: + enum Family { + VnSensor_Family_Unknown, ///< Unknown device family. + VnSensor_Family_Vn100, ///< A device of the VectorNav VN-100 sensor family. + VnSensor_Family_Vn200, ///< A device of the VectorNav VN-200 sensor family. + VnSensor_Family_Vn300 ///< A device of the VectorNav VN-300 sensor family. + }; + + /** \brief The different types of processors. */ + enum VnProcessorType { + VNPROCESSOR_NAV, // Navigation Processor (Main Processor) + VNPROCESSOR_GPS, // GPS Processor + VNPROCESSOR_IMU // IMU Processor + }; + +#if PYTHON + typedef Event AsyncPacketReceivedEvent; +#endif - enum Family - { - VnSensor_Family_Unknown, ///< Unknown device family. - VnSensor_Family_Vn100, ///< A device of the VectorNav VN-100 sensor family. - VnSensor_Family_Vn200, ///< A device of the VectorNav VN-200 sensor family. - VnSensor_Family_Vn300 ///< A device of the VectorNav VN-300 sensor family. - }; - - /** \brief The different types of processors. */ - enum VnProcessorType - { - VNPROCESSOR_NAV, // Navigation Processor (Main Processor) - VNPROCESSOR_GPS, // GPS Processor - VNPROCESSOR_IMU // IMU Processor - }; - - #if PYTHON - typedef Event AsyncPacketReceivedEvent; - #endif - - /// \brief Defines a callback handler that can received notification when - /// the VnSensor receives raw data from its port. - /// - /// \param[in] userData Pointer to user data that was initially supplied - /// when the callback was registered via registerRawDataReceivedHandler. - /// \param[in] rawData Pointer to the raw data. - /// \param[in] length The number of bytes of raw data. - /// \param[in] runningIndex The running index of the received data. - typedef void(*RawDataReceivedHandler)(void* userData, const char* rawData, size_t length, size_t runningIndex); - - /// \brief Defines the signature for a method that can receive - /// notifications of new possible packets found. - /// - /// \param[in] userData Pointer to user data that was initially supplied - /// when the callback was registered via registerPossiblePacketFoundHandler. - /// \param[in] possiblePacket The possible packet that was found. - /// \param[in] packetStartRunningIndex The running index of the start of - /// the packet. - typedef void(*PossiblePacketFoundHandler)(void* userData, protocol::uart::Packet& possiblePacket, size_t packetStartRunningIndex); - - /// \brief Defines the signature for a method that can receive - /// notifications of when a new asynchronous data packet (ASCII or BINARY) - /// is received. - /// - /// This packet will have already had and pertinent error checking - /// performed and determined to be an asynchronous packet. - /// - /// \param[in] userData Pointer to user data that was initially supplied - /// when the callback was registered via registerAsyncPacketReceivedHandler. - /// \param[in] asyncPacket The asynchronous packet received. - /// \param[in] packetStartRunningIndex The running index of the start of - /// the packet. - typedef void(*AsyncPacketReceivedHandler)(void* userData, protocol::uart::Packet& asyncPacket, size_t packetStartRunningIndex); - - /// \brief Defines the signature for a method that can receive - /// notifications when an error message is received. - /// - /// This packet will have already had and pertinent error checking - /// performed and determined to be an asynchronous packet. - /// - /// \param[in] userData Pointer to user data that was initially supplied - /// when the callback was registered via registerErrorPacketReceivedHandler. - /// \param[in] errorPacket The error packet received. - /// \param[in] packetStartRunningIndex The running index of the start of - /// the packet. - typedef void(*ErrorPacketReceivedHandler)(void* userData, protocol::uart::Packet& errorPacket, size_t packetStartRunningIndex); - - /// \brief The list of baudrates supported by VectorNav sensors. - static std::vector supportedBaudrates(); - - VnSensor(); - - ~VnSensor(); - - /// \brief Returns the baudrate of the serial port connection. Note this - /// is independent of the sensor's on-board serial baudrate setting. - /// - /// \return The connected baudrate. - /// \return The connected baudrate. - uint32_t baudrate(); - - /// \brief Returns the name of the port the sensor is connected to. - /// - /// \return The port name. - std::string port(); - - /// \defgroup vnSensorProperties VnSensor Properties - /// \brief This group of methods interface with the VnSensor properties. - /// - /// \{ - - /// \brief Gets the current error detection mode used for commands sent to - /// the sensor. Default is protocol::uart::ErrorDetectionMode::CHECKSUM. - /// - /// \return The error detection mode used for packets sent to the sensor. - protocol::uart::ErrorDetectionMode sendErrorDetectionMode(); - - /// \brief Sets the error detection mode used by the class for commands - /// sent to the sensor. - /// - /// \param[in] mode The new error detection mode for packets sent to the - /// sensor. - void setSendErrorDetectionMode(protocol::uart::ErrorDetectionMode mode); - - /// \brief Indicates if the VnSensor is connected. - /// - /// \return true if the VnSensor is connected; otherwise false. - bool isConnected(); - - /// \brief Gets the amount of time in milliseconds to wait for a response - /// during read/writes with the sensor. - /// - /// \return The response timeout in milliseconds. - uint16_t responseTimeoutMs(); - - /// \brief Sets the amount of time in milliseconds to wait for a response - /// during read/writes with the sensor. - /// - /// \param[in] timeout The number of milliseconds for response timeouts. - void setResponseTimeoutMs(uint16_t timeout); - - /// \brief The delay in milliseconds between retransmitting commands. - /// - /// \return The retransmit delay in milliseconds. - uint16_t retransmitDelayMs(); - - /// \brief Sets the delay in milliseconds between command retransmits. - /// - /// \param[in] delay The retransmit delay in milliseconds. - void setRetransmitDelayMs(uint16_t delay); - - /// \} - - /// \brief Checks if we are able to send and receive communication with a sensor. - /// - /// \return true if we can communicate with the sensor; otherwise false. - bool verifySensorConnectivity(); - - /// \brief Connects to a VectorNav sensor. - /// - /// \param[in] portName The name of the serial port to connect to. - /// \param[in] baudrate The baudrate to connect at. - void connect(const std::string &portName, uint32_t baudrate); - - /// \brief Allows connecting to a VectorNav sensor over a - /// \ref vn::common::ISimplePort. - /// - /// The caller is responsible for properly destroying the - /// \ref vn::common::ISimplePort object when this method is used. Also, if - /// the provided \ref vn::common::ISimplePort is already open, then when - /// the method \ref disconnect is called, \ref VnSensor will not attempt to - /// close the port. However, if the \ref vn::common::ISimplePort is not - /// open, then any subsequent calls to \ref disconnect will close the port. - /// - /// \param[in] simplePort An \ref vn::common::ISimplePort. May be either - /// currently open or closed. - void connect(xplat::IPort* port); - - /// \brief Disconnects from the VectorNav sensor. - /// - /// \exception invalid_operation Thrown if the VnSensor is not - /// connected. - void disconnect(); - - /// \brief Sends the provided command and returns the response from the sensor. - /// - /// If the command does not have an asterisk '*', then a checksum will be performed - /// and appended based on the current error detection mode. Also, if the line-ending - /// \\r\\n is not present, these will be added also. - /// - /// \param[in] toSend The command to send to the sensor. - /// \return The response received from the sensor. - std::string transaction(std::string toSend); - - /// \brief Writes a raw data string to the sensor, normally appending an - /// appropriate error detection checksum. - /// - /// No checksum is necessary as any missing checksum will be provided. For - /// example, the following toSend data will be correctly received by the - /// sensor. - /// - $VNRRG,1*42\\r\\n - /// - $VNRRG,1*42 - /// - $VNRRG,1* - /// - $VNRRG,1 - /// - VNRRG,1 - /// - /// If waitForReply is true, then the method will wait for a - /// response and return the received response. Otherwise, if false, - /// the method will send the data and return immediately with an empty - /// string. - /// - /// \param[in] toSend The data to send. The method will automatically - /// append a checksum/CRC if none is provided. - /// \param[in] waitForReply Indicates if the method should wait for a - /// response and return the received response. - /// \param[in] errorDetectionMode Indicates the error detection mode to - /// append to any packets to send. - /// \return The received response if waitForReply is true; otherwise - /// this will be an empty string. - std::string send( - std::string toSend, - bool waitForReply = true, - protocol::uart::ErrorDetectionMode errorDetectionMode = protocol::uart::ERRORDETECTIONMODE_CHECKSUM); - - /// \brief Issues a tare command to the VectorNav Sensor. - /// - /// \param[in] waitForReply Indicates if the method should wait for a - /// response from the sensor. - void tare(bool waitForReply = true); - - /// \brief Issues a command to the VectorNav Sensor to set the gyro's bias. - /// - /// \param[in] waitForReply Indicates if the method should wait for a - /// response from the sensor. - void setGyroBias(bool waitForReply = true); - - /// \brief Command to inform the VectorNav Sensor if there is a magnetic disturbance present. - /// - /// \param[in] disturbancePresent Indicates the presence of a magnetic disturbance - /// \param[in] waitForReply Indicates if the method should wait for a - /// response from the sensor. - void magneticDisturbancePresent(bool disturbancePresent, bool waitForReply = true); - - /// \brief Command to inform the VectorNav Sensor if there is an acceleration disturbance present. - /// - /// \param[in] disturbancePresent Indicates the presense of an acceleration disturbance. - /// \param[in] waitForReply Indicates if the method should wait for a - /// response from the sensor. - void accelerationDisturbancePresent(bool disturbancePresent, bool waitForReply = true); - - /// \brief Issues a Write Settings command to the VectorNav Sensor. - /// - /// \param[in] waitForReply Indicates if the method should wait for a - /// response from the sensor. - void writeSettings(bool waitForReply = true); - - /// \brief Issues a Restore Factory Settings command to the VectorNav - /// sensor. - /// - /// \param[in] waitForReply Indicates if the method should wait for a - /// response from the sensor. - void restoreFactorySettings(bool waitForReply = true); - - /// \brief Issues a Reset command to the VectorNav sensor. - /// - /// \param[in] waitForReply Indicates if the method should wait for a - /// response from the sensor. - void reset(bool waitForReply = true); - - /// \brief Issues a Firmware Update command to the VectorNav sensor. - /// - /// \param[in] waitForReply Indicates if the method should wait for a - /// response from the sensor. - void firmwareUpdateMode(bool waitForReply = true); - - /// \brief Write a firmware update record to the bootloader - /// - /// \param[in] record The firmware update record to write to the bootloader - /// \param[in] waitForReply Indicates if the method should wait for a - /// response from the sensor. - void writeFirmwareUpdateRecord(const std::string& record, bool waitForReply = true); - - /// \brief Calibrate the bootloader with current baudrate. - /// - /// return string - Bootloader Version Number - std::string calibrateBootloader(); - - /// \brief Issues a change baudrate to the VectorNav sensor and then - /// reconnects the attached serial port at the new baudrate. - /// - /// \param[in] baudrate The new sensor baudrate. - void changeBaudRate(uint32_t baudrate); - - /// \brief This method will connect to the specified serial port, query the - /// attached sensor, and determine the type of device. - /// - /// \param[in] portName The COM port name to connect to. - /// \param[in] baudrate The baudrate to connect at. - //static void determineDeviceFamily(std::string portName, uint32_t baudrate); - - /// \brief This method will query the attached sensor and determine the - /// device family it belongs to. - /// - /// \return The determined device family. - Family determineDeviceFamily(); - - /// \brief This method determines which device family a VectorNav sensor - /// belongs to based on the provided model number. - /// - /// \return The determined device family. - static Family determineDeviceFamily(std::string modelNumber); - - /// \defgroup vnSensorEvents VnSensor Events - /// \brief This group of methods allow registering/unregistering for events - /// of the VnSensor. - /// - /// \{ - - /// \brief Registers a callback method for notification when raw data is - /// received. - /// - /// \param[in] userData Pointer to user data, which will be provided to the - /// callback method. - /// \param[in] handler The callback method. - void registerRawDataReceivedHandler(void* userData, RawDataReceivedHandler handler); - - #if PL150 - //Event event - #else - #if PYTHON - - PyObject* registerRawDataReceivedHandler(PyObject* callable); - - #endif - #endif - - /// \brief Unregisters the registered callback method. - void unregisterRawDataReceivedHandler(); - - /// \brief Registers a callback method for notification when a new possible - /// packet is found. - /// - /// \param[in] userData Pointer to user data, which will be provided to the - /// callback method. - /// \param[in] handler The callback method. - void registerPossiblePacketFoundHandler(void* userData, PossiblePacketFoundHandler handler); - - /// \brief Unregisters the registered callback method. - void unregisterPossiblePacketFoundHandler(); - - /// \brief Registers a callback method for notification when a new - /// asynchronous data packet is received. - /// - /// \param[in] userData Pointer to user data, which will be provided to the - /// callback method. - /// \param[in] handler The callback method. - void registerAsyncPacketReceivedHandler(void* userData, AsyncPacketReceivedHandler handler); - - #if PL150 - //packet, index, timestamp - //Event eventAsyncPacketRecieved; - #if PYTHON - AsyncPacketReceivedEvent eventAsyncPacketReceived; - #endif - - #else - #if PYTHON - PyObject* registerAsyncPacketReceivedHandler(PyObject* callable); - #endif - #endif - - /// \brief Unregisters the registered callback method. - void unregisterAsyncPacketReceivedHandler(); - - /// \brief Registers a callback method for notification when an error - /// packet is received. - /// - /// \param[in] userData Pointer to user data, which will be provided to the - /// callback method. - /// \param[in] handler The callback method. - void registerErrorPacketReceivedHandler(void* userData, ErrorPacketReceivedHandler handler); - - /// \brief Unregisters the registered callback method. - void unregisterErrorPacketReceivedHandler(); - - /// \} - - /// \defgroup registerAccessMethods Register Access Methods - /// \brief This group of methods provide access to read and write to the - /// sensor's registers. - /// - /// \{ - - /// \brief Reads the Binary Output 1 register. - /// - /// \return The register's values. - BinaryOutputRegister readBinaryOutput1(); - - /// \brief Writes to the Binary Output 1 register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeBinaryOutput1(BinaryOutputRegister &fields, bool waitForReply = true); - - /// \brief Reads the Binary Output 2 register. - /// - /// \return The register's values. - BinaryOutputRegister readBinaryOutput2(); - - /// \brief Writes to the Binary Output 2 register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeBinaryOutput2(BinaryOutputRegister &fields, bool waitForReply = true); - - /// \brief Reads the Binary Output 3 register. - /// - /// \return The register's values. - BinaryOutputRegister readBinaryOutput3(); - - /// \brief Writes to the Binary Output 3 register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeBinaryOutput3(BinaryOutputRegister &fields, bool waitForReply = true); - - - /// \brief Reads the Serial Baud Rate register for the specified port. - /// - /// \param[in] port The port number to read from. - /// \return The register's values. - uint32_t readSerialBaudRate(uint8_t port); - - /// \brief Writes to the Serial Baud Rate register for the specified port. - /// - /// \param[in] baudrate The register's Baud Rate field. - /// \param[in] port The port number to write to. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeSerialBaudRate(const uint32_t &baudrate, uint8_t port, bool waitForReply = true); - - /// \brief Reads the Async Data Output Type register. - /// - /// \param[in] port The port number to read from. - /// \return The register's values. - protocol::uart::AsciiAsync readAsyncDataOutputType(uint8_t port); - - /// \brief Writes to the Async Data Output Type register. - /// - /// \param[in] ador The register's ADOR field. - /// \param[in] port The port number to write to. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeAsyncDataOutputType(protocol::uart::AsciiAsync ador, uint8_t port, bool waitForReply = true); - - /// \brief Reads the Async Data Output Frequency register. - /// - /// \param[in] port The port number to read from. - /// \return The register's values. - uint32_t readAsyncDataOutputFrequency(uint8_t port); - - /// \brief Writes to the Async Data Output Frequency register. - /// - /// \param[in] adof The register's ADOF field. - /// \param[in] port The port number to write to. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeAsyncDataOutputFrequency(const uint32_t &adof, uint8_t port, bool waitForReply = true); - - /// \brief Reads the INS Basic Configuration register for a VN-200 sensor. - /// - /// \return The register's values. - InsBasicConfigurationRegisterVn200 readInsBasicConfigurationVn200(); - - /// \brief Writes to the INS Basic Configuration register for a VN-200 sensor. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeInsBasicConfigurationVn200(InsBasicConfigurationRegisterVn200 &fields, bool waitForReply = true); - - /// \brief Writes to the INS Basic Configuration register for a VN-200 sensor. - /// - /// \param[in] scenario Value for the Scenario field. - /// \param[in] ahrsAiding Value for the AhrsAiding field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeInsBasicConfigurationVn200( - protocol::uart::Scenario scenario, - const uint8_t &ahrsAiding, - bool waitForReply = true); - - /// \brief Reads the INS Basic Configuration register for a VN-300 sensor. - /// - /// \return The register's values. - InsBasicConfigurationRegisterVn300 readInsBasicConfigurationVn300(); - - /// \brief Writes to the INS Basic Configuration register for a VN-300 sensor. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeInsBasicConfigurationVn300(InsBasicConfigurationRegisterVn300 &fields, bool waitForReply = true); - - /// \brief Writes to the INS Basic Configuration register for a VN-300 sensor. - /// - /// \param[in] scenario Value for the Scenario field. - /// \param[in] ahrsAiding Value for the AhrsAiding field. - /// \param[in] estBaseline Value for the EstBaseline field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeInsBasicConfigurationVn300( - protocol::uart::Scenario scenario, - const uint8_t &ahrsAiding, - const uint8_t &estBaseline, - bool waitForReply = true); - - /// \brief Reads the User Tag register. - /// - /// \return The register's values. - std::string readUserTag(); - - /// \brief Writes to the User Tag register. - /// - /// \param[in] tag The register's Tag field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeUserTag(const std::string &tag, bool waitForReply = true); - - /// \brief Reads the Model Number register. - /// - /// \return The register's values. - std::string readModelNumber(); - - /// \brief Reads the Hardware Revision register. - /// - /// \return The register's values. - uint32_t readHardwareRevision(); - - /// \brief Reads the Serial Number register. - /// - /// \return The register's values. - uint32_t readSerialNumber(); - - /// \brief Reads the Firmware Version register. - /// - /// \return The register's values. - std::string readFirmwareVersion(); - - /// \brief Reads the Serial Baud Rate register. - /// - /// \return The register's values. - uint32_t readSerialBaudRate(); - - /// \brief Writes to the Serial Baud Rate register. - /// - /// \param[in] baudrate The register's Baud Rate field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeSerialBaudRate(const uint32_t &baudrate, bool waitForReply = true); - - /// \brief Reads the Async Data Output Type register. - /// - /// \return The register's values. - protocol::uart::AsciiAsync readAsyncDataOutputType(); - - /// \brief Writes to the Async Data Output Type register. - /// - /// \param[in] ador The register's ADOR field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeAsyncDataOutputType(protocol::uart::AsciiAsync ador, bool waitForReply = true); - - /// \brief Reads the Async Data Output Frequency register. - /// - /// \return The register's values. - uint32_t readAsyncDataOutputFrequency(); - - /// \brief Writes to the Async Data Output Frequency register. - /// - /// \param[in] adof The register's ADOF field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeAsyncDataOutputFrequency(const uint32_t &adof, bool waitForReply = true); - - /// \brief Reads the Yaw Pitch Roll register. - /// - /// \return The register's values. - vn::math::vec3f readYawPitchRoll(); - - /// \brief Reads the Attitude Quaternion register. - /// - /// \return The register's values. - vn::math::vec4f readAttitudeQuaternion(); - - /// \brief Reads the Quaternion, Magnetic, Acceleration and Angular Rates register. - /// - /// \return The register's values. - QuaternionMagneticAccelerationAndAngularRatesRegister readQuaternionMagneticAccelerationAndAngularRates(); - - /// \brief Reads the Magnetic Measurements register. - /// - /// \return The register's values. - vn::math::vec3f readMagneticMeasurements(); - - /// \brief Reads the Acceleration Measurements register. - /// - /// \return The register's values. - vn::math::vec3f readAccelerationMeasurements(); - - /// \brief Reads the Angular Rate Measurements register. - /// - /// \return The register's values. - vn::math::vec3f readAngularRateMeasurements(); - - /// \brief Reads the Magnetic, Acceleration and Angular Rates register. - /// - /// \return The register's values. - MagneticAccelerationAndAngularRatesRegister readMagneticAccelerationAndAngularRates(); - - /// \brief Reads the Magnetic and Gravity Reference Vectors register. - /// - /// \return The register's values. - MagneticAndGravityReferenceVectorsRegister readMagneticAndGravityReferenceVectors(); - - /// \brief Writes to the Magnetic and Gravity Reference Vectors register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeMagneticAndGravityReferenceVectors(MagneticAndGravityReferenceVectorsRegister &fields, bool waitForReply = true); - - /// \brief Writes to the Magnetic and Gravity Reference Vectors register. - /// - /// \param[in] magRef Value for the MagRef field. - /// \param[in] accRef Value for the AccRef field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeMagneticAndGravityReferenceVectors( - const vn::math::vec3f &magRef, - const vn::math::vec3f &accRef, - bool waitForReply = true); - - /// \brief Reads the Filter Measurements Variance Parameters register. - /// - /// \return The register's values. - FilterMeasurementsVarianceParametersRegister readFilterMeasurementsVarianceParameters(); - - /// \brief Writes to the Filter Measurements Variance Parameters register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeFilterMeasurementsVarianceParameters(FilterMeasurementsVarianceParametersRegister &fields, bool waitForReply = true); - - /// \brief Writes to the Filter Measurements Variance Parameters register. - /// - /// \param[in] angularWalkVariance Value for the Angular Walk Variance field. - /// \param[in] angularRateVariance Value for the Angular Rate Variance field. - /// \param[in] magneticVariance Value for the Magnetic Variance field. - /// \param[in] accelerationVariance Value for the Acceleration Variance field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeFilterMeasurementsVarianceParameters( - const float &angularWalkVariance, - const vn::math::vec3f &angularRateVariance, - const vn::math::vec3f &magneticVariance, - const vn::math::vec3f &accelerationVariance, - bool waitForReply = true); - - /// \brief Reads the Magnetometer Compensation register. - /// - /// \return The register's values. - MagnetometerCompensationRegister readMagnetometerCompensation(); - - /// \brief Writes to the Magnetometer Compensation register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeMagnetometerCompensation(MagnetometerCompensationRegister &fields, bool waitForReply = true); - - /// \brief Writes to the Magnetometer Compensation register. - /// - /// \param[in] c Value for the C field. - /// \param[in] b Value for the B field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeMagnetometerCompensation( - const vn::math::mat3f &c, - const vn::math::vec3f &b, - bool waitForReply = true); - - /// \brief Reads the Filter Active Tuning Parameters register. - /// - /// \return The register's values. - FilterActiveTuningParametersRegister readFilterActiveTuningParameters(); - - /// \brief Writes to the Filter Active Tuning Parameters register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeFilterActiveTuningParameters(FilterActiveTuningParametersRegister &fields, bool waitForReply = true); - - /// \brief Writes to the Filter Active Tuning Parameters register. - /// - /// \param[in] magneticDisturbanceGain Value for the Magnetic Disturbance Gain field. - /// \param[in] accelerationDisturbanceGain Value for the Acceleration Disturbance Gain field. - /// \param[in] magneticDisturbanceMemory Value for the Magnetic Disturbance Memory field. - /// \param[in] accelerationDisturbanceMemory Value for the Acceleration Disturbance Memory field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeFilterActiveTuningParameters( - const float &magneticDisturbanceGain, - const float &accelerationDisturbanceGain, - const float &magneticDisturbanceMemory, - const float &accelerationDisturbanceMemory, - bool waitForReply = true); - - /// \brief Reads the Acceleration Compensation register. - /// - /// \return The register's values. - AccelerationCompensationRegister readAccelerationCompensation(); - - /// \brief Writes to the Acceleration Compensation register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeAccelerationCompensation(AccelerationCompensationRegister &fields, bool waitForReply = true); - - /// \brief Writes to the Acceleration Compensation register. - /// - /// \param[in] c Value for the C field. - /// \param[in] b Value for the B field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeAccelerationCompensation( - const vn::math::mat3f &c, - const vn::math::vec3f &b, - bool waitForReply = true); - - /// \brief Reads the Reference Frame Rotation register. - /// - /// \return The register's values. - vn::math::mat3f readReferenceFrameRotation(); - - /// \brief Writes to the Reference Frame Rotation register. - /// - /// \param[in] c The register's C field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeReferenceFrameRotation(const vn::math::mat3f &c, bool waitForReply = true); - - /// \brief Reads the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register. - /// - /// \return The register's values. - YawPitchRollMagneticAccelerationAndAngularRatesRegister readYawPitchRollMagneticAccelerationAndAngularRates(); - - /// \brief Reads the Communication Protocol Control register. - /// - /// \return The register's values. - CommunicationProtocolControlRegister readCommunicationProtocolControl(); - - /// \brief Writes to the Communication Protocol Control register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeCommunicationProtocolControl(CommunicationProtocolControlRegister &fields, bool waitForReply = true); - - /// \brief Writes to the Communication Protocol Control register. - /// - /// \param[in] serialCount Value for the SerialCount field. - /// \param[in] serialStatus Value for the SerialStatus field. - /// \param[in] spiCount Value for the SPICount field. - /// \param[in] spiStatus Value for the SPIStatus field. - /// \param[in] serialChecksum Value for the SerialChecksum field. - /// \param[in] spiChecksum Value for the SPIChecksum field. - /// \param[in] errorMode Value for the ErrorMode field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeCommunicationProtocolControl( - protocol::uart::CountMode serialCount, - protocol::uart::StatusMode serialStatus, - protocol::uart::CountMode spiCount, - protocol::uart::StatusMode spiStatus, - protocol::uart::ChecksumMode serialChecksum, - protocol::uart::ChecksumMode spiChecksum, - protocol::uart::ErrorMode errorMode, - bool waitForReply = true); - - /// \brief Reads the Synchronization Control register. - /// - /// \return The register's values. - SynchronizationControlRegister readSynchronizationControl(); - - /// \brief Writes to the Synchronization Control register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeSynchronizationControl(SynchronizationControlRegister &fields, bool waitForReply = true); - - /// \brief Writes to the Synchronization Control register. - /// - /// \param[in] syncInMode Value for the SyncInMode field. - /// \param[in] syncInEdge Value for the SyncInEdge field. - /// \param[in] syncInSkipFactor Value for the SyncInSkipFactor field. - /// \param[in] syncOutMode Value for the SyncOutMode field. - /// \param[in] syncOutPolarity Value for the SyncOutPolarity field. - /// \param[in] syncOutSkipFactor Value for the SyncOutSkipFactor field. - /// \param[in] syncOutPulseWidth Value for the SyncOutPulseWidth field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeSynchronizationControl( - protocol::uart::SyncInMode syncInMode, - protocol::uart::SyncInEdge syncInEdge, - const uint16_t &syncInSkipFactor, - protocol::uart::SyncOutMode syncOutMode, - protocol::uart::SyncOutPolarity syncOutPolarity, - const uint16_t &syncOutSkipFactor, - const uint32_t &syncOutPulseWidth, - bool waitForReply = true); - - /// \brief Reads the Synchronization Status register. - /// - /// \return The register's values. - SynchronizationStatusRegister readSynchronizationStatus(); - - /// \brief Writes to the Synchronization Status register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeSynchronizationStatus(SynchronizationStatusRegister &fields, bool waitForReply = true); - - /// \brief Writes to the Synchronization Status register. - /// - /// \param[in] syncInCount Value for the SyncInCount field. - /// \param[in] syncInTime Value for the SyncInTime field. - /// \param[in] syncOutCount Value for the SyncOutCount field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeSynchronizationStatus( - const uint32_t &syncInCount, - const uint32_t &syncInTime, - const uint32_t &syncOutCount, - bool waitForReply = true); - - /// \brief Reads the Filter Basic Control register. - /// - /// \return The register's values. - FilterBasicControlRegister readFilterBasicControl(); - - /// \brief Writes to the Filter Basic Control register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeFilterBasicControl(FilterBasicControlRegister &fields, bool waitForReply = true); - - /// \brief Writes to the Filter Basic Control register. - /// - /// \param[in] magMode Value for the MagMode field. - /// \param[in] extMagMode Value for the ExtMagMode field. - /// \param[in] extAccMode Value for the ExtAccMode field. - /// \param[in] extGyroMode Value for the ExtGyroMode field. - /// \param[in] gyroLimit Value for the GyroLimit field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeFilterBasicControl( - protocol::uart::MagneticMode magMode, - protocol::uart::ExternalSensorMode extMagMode, - protocol::uart::ExternalSensorMode extAccMode, - protocol::uart::ExternalSensorMode extGyroMode, - const vn::math::vec3f &gyroLimit, - bool waitForReply = true); - - /// \brief Reads the Heave Configuration register. - /// - /// \return The register's values. - HeaveConfigurationRegister readHeaveConfiguration(); - - /// \brief Writes to the Heave Configuration register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeHeaveConfiguration(HeaveConfigurationRegister &fields, bool waitForReply = true); - - /// \brief Writes to the Heave Configuration register. - /// - /// \param[in] initialWavePeriod Value for the initialWavePeriod field. - /// \param[in] initialWaveAmplitude Value for the initialWaveAmplitude field. - /// \param[in] maxWavePeriod Value for the maxWavePeriod field. - /// \param[in] minWaveAmplitude Value for the minWaveAmplitude field. - /// \param[in] delayedHeaveCutoffFreq Value for the delayedHeaveCutoffFreq field. - /// \param[in] heaveCutoffFreq Value for the heaveCutoffFreq field. - /// \param[in] heaveRateCutoffFreq Value for the heaveRateCutoffFreq field. - void writeHeaveConfiguration( - const float &initialWavePeriod, - const float &initialWaveAmplitude, - const float &maxWavePeriod, - const float &minWaveAmplitude, - const float &delayedHeaveCutoffFreq, - const float &heaveCutoffFreq, - const float &heaveRateCutoffFreq, - bool waitForReply = true); - - /// \brief Reads the VPE Basic Control register. - /// - /// \return The register's values. - VpeBasicControlRegister readVpeBasicControl(); - - /// \brief Writes to the VPE Basic Control register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeVpeBasicControl(VpeBasicControlRegister &fields, bool waitForReply = true); - - /// \brief Writes to the VPE Basic Control register. - /// - /// \param[in] enable Value for the Enable field. - /// \param[in] headingMode Value for the HeadingMode field. - /// \param[in] filteringMode Value for the FilteringMode field. - /// \param[in] tuningMode Value for the TuningMode field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeVpeBasicControl( - protocol::uart::VpeEnable enable, - protocol::uart::HeadingMode headingMode, - protocol::uart::VpeMode filteringMode, - protocol::uart::VpeMode tuningMode, - bool waitForReply = true); - - /// \brief Reads the VPE Magnetometer Basic Tuning register. - /// - /// \return The register's values. - VpeMagnetometerBasicTuningRegister readVpeMagnetometerBasicTuning(); - - /// \brief Writes to the VPE Magnetometer Basic Tuning register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeVpeMagnetometerBasicTuning(VpeMagnetometerBasicTuningRegister &fields, bool waitForReply = true); - - /// \brief Writes to the VPE Magnetometer Basic Tuning register. - /// - /// \param[in] baseTuning Value for the BaseTuning field. - /// \param[in] adaptiveTuning Value for the AdaptiveTuning field. - /// \param[in] adaptiveFiltering Value for the AdaptiveFiltering field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeVpeMagnetometerBasicTuning( - const vn::math::vec3f &baseTuning, - const vn::math::vec3f &adaptiveTuning, - const vn::math::vec3f &adaptiveFiltering, - bool waitForReply = true); - - /// \brief Reads the VPE Magnetometer Advanced Tuning register. - /// - /// \return The register's values. - VpeMagnetometerAdvancedTuningRegister readVpeMagnetometerAdvancedTuning(); - - /// \brief Writes to the VPE Magnetometer Advanced Tuning register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeVpeMagnetometerAdvancedTuning(VpeMagnetometerAdvancedTuningRegister &fields, bool waitForReply = true); - - /// \brief Writes to the VPE Magnetometer Advanced Tuning register. - /// - /// \param[in] minFiltering Value for the MinFiltering field. - /// \param[in] maxFiltering Value for the MaxFiltering field. - /// \param[in] maxAdaptRate Value for the MaxAdaptRate field. - /// \param[in] disturbanceWindow Value for the DisturbanceWindow field. - /// \param[in] maxTuning Value for the MaxTuning field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeVpeMagnetometerAdvancedTuning( - const vn::math::vec3f &minFiltering, - const vn::math::vec3f &maxFiltering, - const float &maxAdaptRate, - const float &disturbanceWindow, - const float &maxTuning, - bool waitForReply = true); - - /// \brief Reads the VPE Accelerometer Basic Tuning register. - /// - /// \return The register's values. - VpeAccelerometerBasicTuningRegister readVpeAccelerometerBasicTuning(); - - /// \brief Writes to the VPE Accelerometer Basic Tuning register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeVpeAccelerometerBasicTuning(VpeAccelerometerBasicTuningRegister &fields, bool waitForReply = true); - - /// \brief Writes to the VPE Accelerometer Basic Tuning register. - /// - /// \param[in] baseTuning Value for the BaseTuning field. - /// \param[in] adaptiveTuning Value for the AdaptiveTuning field. - /// \param[in] adaptiveFiltering Value for the AdaptiveFiltering field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeVpeAccelerometerBasicTuning( - const vn::math::vec3f &baseTuning, - const vn::math::vec3f &adaptiveTuning, - const vn::math::vec3f &adaptiveFiltering, - bool waitForReply = true); - - /// \brief Reads the VPE Accelerometer Advanced Tuning register. - /// - /// \return The register's values. - VpeAccelerometerAdvancedTuningRegister readVpeAccelerometerAdvancedTuning(); - - /// \brief Writes to the VPE Accelerometer Advanced Tuning register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeVpeAccelerometerAdvancedTuning(VpeAccelerometerAdvancedTuningRegister &fields, bool waitForReply = true); - - /// \brief Writes to the VPE Accelerometer Advanced Tuning register. - /// - /// \param[in] minFiltering Value for the MinFiltering field. - /// \param[in] maxFiltering Value for the MaxFiltering field. - /// \param[in] maxAdaptRate Value for the MaxAdaptRate field. - /// \param[in] disturbanceWindow Value for the DisturbanceWindow field. - /// \param[in] maxTuning Value for the MaxTuning field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeVpeAccelerometerAdvancedTuning( - const vn::math::vec3f &minFiltering, - const vn::math::vec3f &maxFiltering, - const float &maxAdaptRate, - const float &disturbanceWindow, - const float &maxTuning, - bool waitForReply = true); - - /// \brief Reads the VPE Gyro Basic Tuning register. - /// - /// \return The register's values. - VpeGyroBasicTuningRegister readVpeGyroBasicTuning(); - - /// \brief Writes to the VPE Gyro Basic Tuning register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeVpeGyroBasicTuning(VpeGyroBasicTuningRegister &fields, bool waitForReply = true); - - /// \brief Writes to the VPE Gyro Basic Tuning register. - /// - /// \param[in] angularWalkVariance Value for the AngularWalkVariance field. - /// \param[in] baseTuning Value for the BaseTuning field. - /// \param[in] adaptiveTuning Value for the AdaptiveTuning field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeVpeGyroBasicTuning( - const vn::math::vec3f &angularWalkVariance, - const vn::math::vec3f &baseTuning, - const vn::math::vec3f &adaptiveTuning, - bool waitForReply = true); - - /// \brief Reads the Filter Startup Gyro Bias register. - /// - /// \return The register's values. - vn::math::vec3f readFilterStartupGyroBias(); - - /// \brief Writes to the Filter Startup Gyro Bias register. - /// - /// \param[in] bias The register's Bias field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeFilterStartupGyroBias(const vn::math::vec3f &bias, bool waitForReply = true); - - /// \brief Reads the Magnetometer Calibration Control register. - /// - /// \return The register's values. - MagnetometerCalibrationControlRegister readMagnetometerCalibrationControl(); - - /// \brief Writes to the Magnetometer Calibration Control register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeMagnetometerCalibrationControl(MagnetometerCalibrationControlRegister &fields, bool waitForReply = true); - - /// \brief Writes to the Magnetometer Calibration Control register. - /// - /// \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] waitForReply Indicates if the method should wait for a response from the sensor. - void writeMagnetometerCalibrationControl( - protocol::uart::HsiMode hsiMode, - protocol::uart::HsiOutput hsiOutput, - const uint8_t &convergeRate, - bool waitForReply = true); - - /// \brief Reads the Calculated Magnetometer Calibration register. - /// - /// \return The register's values. - CalculatedMagnetometerCalibrationRegister readCalculatedMagnetometerCalibration(); - - /// \brief Reads the Indoor Heading Mode Control register. - /// - /// \return The register's values. - float readIndoorHeadingModeControl(); - - /// \brief Writes to the Indoor Heading Mode Control register. - /// - /// \param[in] maxRateError The register's Max Rate Error field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeIndoorHeadingModeControl(const float &maxRateError, bool waitForReply = true); - - /// \brief Reads the Velocity Compensation Measurement register. - /// - /// \return The register's values. - vn::math::vec3f readVelocityCompensationMeasurement(); - - /// \brief Writes to the Velocity Compensation Measurement register. - /// - /// \param[in] velocity The register's Velocity field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeVelocityCompensationMeasurement(const vn::math::vec3f &velocity, bool waitForReply = true); - - /// \brief Reads the Velocity Compensation Control register. - /// - /// \return The register's values. - VelocityCompensationControlRegister readVelocityCompensationControl(); - - /// \brief Writes to the Velocity Compensation Control register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeVelocityCompensationControl(VelocityCompensationControlRegister &fields, bool waitForReply = true); - - /// \brief Writes to the Velocity Compensation Control register. - /// - /// \param[in] mode Value for the Mode field. - /// \param[in] velocityTuning Value for the VelocityTuning field. - /// \param[in] rateTuning Value for the RateTuning field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeVelocityCompensationControl( - protocol::uart::VelocityCompensationMode mode, - const float &velocityTuning, - const float &rateTuning, - bool waitForReply = true); - - /// \brief Reads the Velocity Compensation Status register. - /// - /// \return The register's values. - VelocityCompensationStatusRegister readVelocityCompensationStatus(); - - /// \brief Reads the IMU Measurements register. - /// - /// \return The register's values. - ImuMeasurementsRegister readImuMeasurements(); - - /// \brief Reads the GPS Configuration register. - /// - /// \return The register's values. - GpsConfigurationRegister readGpsConfiguration(); - - /// \brief Writes to the GPS Configuration register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeGpsConfiguration(GpsConfigurationRegister &fields, bool waitForReply = true); - - /// \brief Writes to the GPS Configuration register (deprecated). - /// - /// \param[in] mode Value for the Mode field. - /// \param[in] ppsSource Value for the PpsSource field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeGpsConfiguration( - protocol::uart::GpsMode mode, - protocol::uart::PpsSource ppsSource, - bool waitForReply = true); - - /// \brief Writes to the GPS Configuration register. - /// - /// \param[in] mode Value for the Mode field. - /// \param[in] ppsSource Value for the PpsSource field. - /// \param[in] rate Value for the Rate field. - /// \param[in] antPow Value for the AntPow field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeGpsConfiguration( - protocol::uart::GpsMode mode, - protocol::uart::PpsSource ppsSource, - protocol::uart::GpsRate rate, - protocol::uart::AntPower antPow, - bool waitForReply = true); - - /// \brief Reads the GPS Antenna Offset register. - /// - /// \return The register's values. - vn::math::vec3f readGpsAntennaOffset(); - - /// \brief Writes to the GPS Antenna Offset register. - /// - /// \param[in] position The register's Position field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeGpsAntennaOffset(const vn::math::vec3f &position, bool waitForReply = true); - - /// \brief Reads the GPS Solution - LLA register. - /// - /// \return The register's values. - GpsSolutionLlaRegister readGpsSolutionLla(); - - /// \brief Reads the GPS Solution - ECEF register. - /// - /// \return The register's values. - GpsSolutionEcefRegister readGpsSolutionEcef(); - - /// \brief Reads the INS Solution - LLA register. - /// - /// \return The register's values. - InsSolutionLlaRegister readInsSolutionLla(); - - /// \brief Reads the INS Solution - ECEF register. - /// - /// \return The register's values. - InsSolutionEcefRegister readInsSolutionEcef(); - - /// \brief Reads the INS Advanced Configuration register. - /// - /// \return The register's values. - InsAdvancedConfigurationRegister readInsAdvancedConfiguration(); - - /// \brief Writes to the INS Advanced Configuration register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeInsAdvancedConfiguration(InsAdvancedConfigurationRegister &fields, bool waitForReply = true); - - /// \brief Writes to the INS Advanced Configuration register. - /// - /// \param[in] useMag Value for the UseMag field. - /// \param[in] usePres Value for the UsePres field. - /// \param[in] posAtt Value for the PosAtt field. - /// \param[in] velAtt Value for the VelAtt field. - /// \param[in] velBias Value for the VelBias field. - /// \param[in] useFoam Value for the UseFoam field. - /// \param[in] gpsCovType Value for the GPSCovType field. - /// \param[in] velCount Value for the VelCount field. - /// \param[in] velInit Value for the VelInit field. - /// \param[in] moveOrigin Value for the MoveOrigin field. - /// \param[in] gpsTimeout Value for the GPSTimeout field. - /// \param[in] deltaLimitPos Value for the DeltaLimitPos field. - /// \param[in] deltaLimitVel Value for the DeltaLimitVel field. - /// \param[in] minPosUncertainty Value for the MinPosUncertainty field. - /// \param[in] minVelUncertainty Value for the MinVelUncertainty field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeInsAdvancedConfiguration( - const uint8_t &useMag, - const uint8_t &usePres, - const uint8_t &posAtt, - const uint8_t &velAtt, - const uint8_t &velBias, - protocol::uart::FoamInit useFoam, - const uint8_t &gpsCovType, - const uint8_t &velCount, - const float &velInit, - const float &moveOrigin, - const float &gpsTimeout, - const float &deltaLimitPos, - const float &deltaLimitVel, - const float &minPosUncertainty, - const float &minVelUncertainty, - bool waitForReply = true); - - /// \brief Reads the INS State - LLA register. - /// - /// \return The register's values. - InsStateLlaRegister readInsStateLla(); - - /// \brief Reads the INS State - ECEF register. - /// - /// \return The register's values. - InsStateEcefRegister readInsStateEcef(); - - /// \brief Reads the Startup Filter Bias Estimate register. - /// - /// \return The register's values. - StartupFilterBiasEstimateRegister readStartupFilterBiasEstimate(); - - /// \brief Writes to the Startup Filter Bias Estimate register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeStartupFilterBiasEstimate(StartupFilterBiasEstimateRegister &fields, bool waitForReply = true); - - /// \brief Writes to the Startup Filter Bias Estimate register. - /// - /// \param[in] gyroBias Value for the GyroBias field. - /// \param[in] accelBias Value for the AccelBias field. - /// \param[in] pressureBias Value for the PressureBias field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeStartupFilterBiasEstimate( - const vn::math::vec3f &gyroBias, - const vn::math::vec3f &accelBias, - const float &pressureBias, - bool waitForReply = true); - - /// \brief Reads the Delta Theta and Delta Velocity register. - /// - /// \return The register's values. - DeltaThetaAndDeltaVelocityRegister readDeltaThetaAndDeltaVelocity(); - - /// \brief Reads the Delta Theta and Delta Velocity Configuration register. - /// - /// \return The register's values. - DeltaThetaAndDeltaVelocityConfigurationRegister readDeltaThetaAndDeltaVelocityConfiguration(); - - /// \brief Writes to the Delta Theta and Delta Velocity Configuration register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeDeltaThetaAndDeltaVelocityConfiguration(DeltaThetaAndDeltaVelocityConfigurationRegister &fields, bool waitForReply = true); - - /// \brief Writes to the Delta Theta and Delta Velocity Configuration register (deprecated). - /// - /// \param[in] integrationFrame Value for the IntegrationFrame field. - /// \param[in] gyroCompensation Value for the GyroCompensation field. - /// \param[in] accelCompensation Value for the AccelCompensation field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeDeltaThetaAndDeltaVelocityConfiguration( - protocol::uart::IntegrationFrame integrationFrame, - protocol::uart::CompensationMode gyroCompensation, - protocol::uart::AccCompensationMode accelCompensation, - bool waitForReply = true); - - /// \brief Writes to the Delta Theta and Delta Velocity Configuration register. - /// - /// \param[in] integrationFrame Value for the IntegrationFrame field. - /// \param[in] gyroCompensation Value for the GyroCompensation field. - /// \param[in] accelCompensation Value for the AccelCompensation field. - /// \param[in] earthRateCorrection Value for the EarthRateCorrection field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeDeltaThetaAndDeltaVelocityConfiguration( - protocol::uart::IntegrationFrame integrationFrame, - protocol::uart::CompensationMode gyroCompensation, - protocol::uart::AccCompensationMode accelCompensation, - protocol::uart::EarthRateCorrection earthRateCorrection, - bool waitForReply = true); - - /// \brief Reads the Reference Vector Configuration register. - /// - /// \return The register's values. - ReferenceVectorConfigurationRegister readReferenceVectorConfiguration(); - - /// \brief Writes to the Reference Vector Configuration register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeReferenceVectorConfiguration(ReferenceVectorConfigurationRegister &fields, bool waitForReply = true); - - /// \brief Writes to the Reference Vector Configuration register. - /// - /// \param[in] useMagModel Value for the UseMagModel field. - /// \param[in] useGravityModel Value for the UseGravityModel field. - /// \param[in] recalcThreshold Value for the RecalcThreshold field. - /// \param[in] year Value for the Year field. - /// \param[in] position Value for the Position field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeReferenceVectorConfiguration( - const uint8_t &useMagModel, - const uint8_t &useGravityModel, - const uint32_t &recalcThreshold, - const float &year, - const vn::math::vec3d &position, - bool waitForReply = true); - - /// \brief Reads the Gyro Compensation register. - /// - /// \return The register's values. - GyroCompensationRegister readGyroCompensation(); - - /// \brief Writes to the Gyro Compensation register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeGyroCompensation(GyroCompensationRegister &fields, bool waitForReply = true); - - /// \brief Writes to the Gyro Compensation register. - /// - /// \param[in] c Value for the C field. - /// \param[in] b Value for the B field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeGyroCompensation( - const vn::math::mat3f &c, - const vn::math::vec3f &b, - bool waitForReply = true); - - /// \brief Reads the IMU Filtering Configuration register. - /// - /// \return The register's values. - ImuFilteringConfigurationRegister readImuFilteringConfiguration(); - - /// \brief Writes to the IMU Filtering Configuration register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeImuFilteringConfiguration(ImuFilteringConfigurationRegister &fields, bool waitForReply = true); - - /// \brief Writes to the IMU Filtering Configuration register. - /// - /// \param[in] magWindowSize Value for the MagWindowSize field. - /// \param[in] accelWindowSize Value for the AccelWindowSize field. - /// \param[in] gyroWindowSize Value for the GyroWindowSize field. - /// \param[in] tempWindowSize Value for the TempWindowSize field. - /// \param[in] presWindowSize Value for the PresWindowSize field. - /// \param[in] magFilterMode Value for the MagFilterMode field. - /// \param[in] accelFilterMode Value for the AccelFilterMode field. - /// \param[in] gyroFilterMode Value for the GyroFilterMode field. - /// \param[in] tempFilterMode Value for the TempFilterMode field. - /// \param[in] presFilterMode Value for the PresFilterMode field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeImuFilteringConfiguration( - const uint16_t &magWindowSize, - const uint16_t &accelWindowSize, - const uint16_t &gyroWindowSize, - const uint16_t &tempWindowSize, - const uint16_t &presWindowSize, - protocol::uart::FilterMode magFilterMode, - protocol::uart::FilterMode accelFilterMode, - protocol::uart::FilterMode gyroFilterMode, - protocol::uart::FilterMode tempFilterMode, - protocol::uart::FilterMode presFilterMode, - bool waitForReply = true); - - /// \brief Reads the GPS Compass Baseline register. - /// - /// \return The register's values. - GpsCompassBaselineRegister readGpsCompassBaseline(); - - /// \brief Writes to the GPS Compass Baseline register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeGpsCompassBaseline(GpsCompassBaselineRegister &fields, bool waitForReply = true); - - /// \brief Writes to the GPS Compass Baseline register. - /// - /// \param[in] position Value for the Position field. - /// \param[in] uncertainty Value for the Uncertainty field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeGpsCompassBaseline( - const vn::math::vec3f &position, - const vn::math::vec3f &uncertainty, - bool waitForReply = true); - - /// \brief Reads the GPS Compass Estimated Baseline register. - /// - /// \return The register's values. - GpsCompassEstimatedBaselineRegister readGpsCompassEstimatedBaseline(); - - /// \brief Reads the IMU Rate Configuration register. - /// - /// \return The register's values. - ImuRateConfigurationRegister readImuRateConfiguration(); - - /// \brief Writes to the IMU Rate Configuration register. - /// - /// \param[in] fields The register's fields. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeImuRateConfiguration(ImuRateConfigurationRegister &fields, bool waitForReply = true); - - /// \brief Writes to the IMU Rate Configuration register. - /// - /// \param[in] imuRate Value for the imuRate field. - /// \param[in] navDivisor Value for the NavDivisor field. - /// \param[in] filterTargetRate Value for the filterTargetRate field. - /// \param[in] filterMinRate Value for the filterMinRate field. - /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. - void writeImuRateConfiguration( - const uint16_t &imuRate, - const uint16_t &navDivisor, - const float &filterTargetRate, - const float &filterMinRate, - bool waitForReply = true); - - /// \brief Reads the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register. - /// - /// \return The register's values. - YawPitchRollTrueBodyAccelerationAndAngularRatesRegister readYawPitchRollTrueBodyAccelerationAndAngularRates(); - - /// \brief Reads the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register. - /// - /// \return The register's values. - YawPitchRollTrueInertialAccelerationAndAngularRatesRegister readYawPitchRollTrueInertialAccelerationAndAngularRates(); - - /// \brief Upgrade the connected sensor with the supplied firmware file - /// - /// \param[in] processor The target processor to switch to. - /// \param[in] model The model of sensor. - /// \param[in] firmware The current firmware version of sensor. - void switchProcessors(VnProcessorType processor, std::string model, std::string firmware); - - /// \brief Upgrade the connected sensor with the supplied firmware file - /// - /// \param]in] portName The baud rate used to update the firmware. (Can be different than the current baud rate) - /// \param[in] filename The path to the VNX firmware file for the sensor. - void firmwareUpdate(int baudRate, std::string fileName); - - /// \brief Upgrade the connected sensor with the supplied firmware file - /// - /// \param]in] portName The baud rate used to update the firmware. (Can be different than the current baud rate) - void setFirmwareUpdateBaudRate(int baudRate); - - /// \brief Get the next Firmware Update Record - /// - /// \return The next record in the Firmware Update file, else an empty string - std::string getNextFirwareUpdateRecord(); - - /// \brief Open the firmware update file - void openFirmwareUpdateFile(std::string filename); - - /// \brief Close the firmware update file - void closeFirmwareUpdateFile(); - - - /// \} - - #ifdef PYTHON_WRAPPERS - - #endif - - #if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 - - void stopRequest(); - bool threadStopped(); - void unregisterListners(); - void shutdownRequest(); - void goRequest(); - - #endif + /// \brief Defines a callback handler that can received notification when + /// the VnSensor receives raw data from its port. + /// + /// \param[in] userData Pointer to user data that was initially supplied + /// when the callback was registered via registerRawDataReceivedHandler. + /// \param[in] rawData Pointer to the raw data. + /// \param[in] length The number of bytes of raw data. + /// \param[in] runningIndex The running index of the received data. + typedef void (*RawDataReceivedHandler)( + void * userData, const char * rawData, size_t length, size_t runningIndex); + + /// \brief Defines the signature for a method that can receive + /// notifications of new possible packets found. + /// + /// \param[in] userData Pointer to user data that was initially supplied + /// when the callback was registered via registerPossiblePacketFoundHandler. + /// \param[in] possiblePacket The possible packet that was found. + /// \param[in] packetStartRunningIndex The running index of the start of + /// the packet. + typedef void (*PossiblePacketFoundHandler)( + void * userData, protocol::uart::Packet & possiblePacket, size_t packetStartRunningIndex); + + /// \brief Defines the signature for a method that can receive + /// notifications of when a new asynchronous data packet (ASCII or BINARY) + /// is received. + /// + /// This packet will have already had and pertinent error checking + /// performed and determined to be an asynchronous packet. + /// + /// \param[in] userData Pointer to user data that was initially supplied + /// when the callback was registered via registerAsyncPacketReceivedHandler. + /// \param[in] asyncPacket The asynchronous packet received. + /// \param[in] packetStartRunningIndex The running index of the start of + /// the packet. + typedef void (*AsyncPacketReceivedHandler)( + void * userData, protocol::uart::Packet & asyncPacket, size_t packetStartRunningIndex); + + /// \brief Defines the signature for a method that can receive + /// notifications when an error message is received. + /// + /// This packet will have already had and pertinent error checking + /// performed and determined to be an asynchronous packet. + /// + /// \param[in] userData Pointer to user data that was initially supplied + /// when the callback was registered via registerErrorPacketReceivedHandler. + /// \param[in] errorPacket The error packet received. + /// \param[in] packetStartRunningIndex The running index of the start of + /// the packet. + typedef void (*ErrorPacketReceivedHandler)( + void * userData, protocol::uart::Packet & errorPacket, size_t packetStartRunningIndex); + + /// \brief The list of baudrates supported by VectorNav sensors. + static std::vector supportedBaudrates(); + + VnSensor(); + + ~VnSensor(); + + /// \brief Returns the baudrate of the serial port connection. Note this + /// is independent of the sensor's on-board serial baudrate setting. + /// + /// \return The connected baudrate. + /// \return The connected baudrate. + uint32_t baudrate(); + + /// \brief Returns the name of the port the sensor is connected to. + /// + /// \return The port name. + std::string port(); + + /// \defgroup vnSensorProperties VnSensor Properties + /// \brief This group of methods interface with the VnSensor properties. + /// + /// \{ + + /// \brief Gets the current error detection mode used for commands sent to + /// the sensor. Default is protocol::uart::ErrorDetectionMode::CHECKSUM. + /// + /// \return The error detection mode used for packets sent to the sensor. + protocol::uart::ErrorDetectionMode sendErrorDetectionMode(); + + /// \brief Sets the error detection mode used by the class for commands + /// sent to the sensor. + /// + /// \param[in] mode The new error detection mode for packets sent to the + /// sensor. + void setSendErrorDetectionMode(protocol::uart::ErrorDetectionMode mode); + + /// \brief Indicates if the VnSensor is connected. + /// + /// \return true if the VnSensor is connected; otherwise false. + bool isConnected(); + + /// \brief Gets the amount of time in milliseconds to wait for a response + /// during read/writes with the sensor. + /// + /// \return The response timeout in milliseconds. + uint16_t responseTimeoutMs(); + + /// \brief Sets the amount of time in milliseconds to wait for a response + /// during read/writes with the sensor. + /// + /// \param[in] timeout The number of milliseconds for response timeouts. + void setResponseTimeoutMs(uint16_t timeout); + + /// \brief The delay in milliseconds between retransmitting commands. + /// + /// \return The retransmit delay in milliseconds. + uint16_t retransmitDelayMs(); + + /// \brief Sets the delay in milliseconds between command retransmits. + /// + /// \param[in] delay The retransmit delay in milliseconds. + void setRetransmitDelayMs(uint16_t delay); + + /// \} + + /// \brief Checks if we are able to send and receive communication with a sensor. + /// + /// \return true if we can communicate with the sensor; otherwise false. + bool verifySensorConnectivity(); + + /// \brief Connects to a VectorNav sensor. + /// + /// \param[in] portName The name of the serial port to connect to. + /// \param[in] baudrate The baudrate to connect at. + void connect(const std::string & portName, uint32_t baudrate); + + /// \brief Allows connecting to a VectorNav sensor over a + /// \ref vn::common::ISimplePort. + /// + /// The caller is responsible for properly destroying the + /// \ref vn::common::ISimplePort object when this method is used. Also, if + /// the provided \ref vn::common::ISimplePort is already open, then when + /// the method \ref disconnect is called, \ref VnSensor will not attempt to + /// close the port. However, if the \ref vn::common::ISimplePort is not + /// open, then any subsequent calls to \ref disconnect will close the port. + /// + /// \param[in] simplePort An \ref vn::common::ISimplePort. May be either + /// currently open or closed. + void connect(xplat::IPort * port); + + /// \brief Disconnects from the VectorNav sensor. + /// + /// \exception invalid_operation Thrown if the VnSensor is not + /// connected. + void disconnect(); + + /// \brief Sends the provided command and returns the response from the sensor. + /// + /// If the command does not have an asterisk '*', then a checksum will be performed + /// and appended based on the current error detection mode. Also, if the line-ending + /// \\r\\n is not present, these will be added also. + /// + /// \param[in] toSend The command to send to the sensor. + /// \return The response received from the sensor. + std::string transaction(std::string toSend); + + /// \brief Writes a raw data string to the sensor, normally appending an + /// appropriate error detection checksum. + /// + /// No checksum is necessary as any missing checksum will be provided. For + /// example, the following toSend data will be correctly received by the + /// sensor. + /// - $VNRRG,1*42\\r\\n + /// - $VNRRG,1*42 + /// - $VNRRG,1* + /// - $VNRRG,1 + /// - VNRRG,1 + /// + /// If waitForReply is true, then the method will wait for a + /// response and return the received response. Otherwise, if false, + /// the method will send the data and return immediately with an empty + /// string. + /// + /// \param[in] toSend The data to send. The method will automatically + /// append a checksum/CRC if none is provided. + /// \param[in] waitForReply Indicates if the method should wait for a + /// response and return the received response. + /// \param[in] errorDetectionMode Indicates the error detection mode to + /// append to any packets to send. + /// \return The received response if waitForReply is true; otherwise + /// this will be an empty string. + std::string send( + std::string toSend, bool waitForReply = true, + protocol::uart::ErrorDetectionMode errorDetectionMode = + protocol::uart::ERRORDETECTIONMODE_CHECKSUM); + + /// \brief Issues a tare command to the VectorNav Sensor. + /// + /// \param[in] waitForReply Indicates if the method should wait for a + /// response from the sensor. + void tare(bool waitForReply = true); + + /// \brief Issues a command to the VectorNav Sensor to set the gyro's bias. + /// + /// \param[in] waitForReply Indicates if the method should wait for a + /// response from the sensor. + void setGyroBias(bool waitForReply = true); + + /// \brief Command to inform the VectorNav Sensor if there is a magnetic disturbance present. + /// + /// \param[in] disturbancePresent Indicates the presence of a magnetic disturbance + /// \param[in] waitForReply Indicates if the method should wait for a + /// response from the sensor. + void magneticDisturbancePresent(bool disturbancePresent, bool waitForReply = true); + + /// \brief Command to inform the VectorNav Sensor if there is an acceleration disturbance present. + /// + /// \param[in] disturbancePresent Indicates the presense of an acceleration disturbance. + /// \param[in] waitForReply Indicates if the method should wait for a + /// response from the sensor. + void accelerationDisturbancePresent(bool disturbancePresent, bool waitForReply = true); + + /// \brief Issues a Write Settings command to the VectorNav Sensor. + /// + /// \param[in] waitForReply Indicates if the method should wait for a + /// response from the sensor. + void writeSettings(bool waitForReply = true); + + /// \brief Issues a Restore Factory Settings command to the VectorNav + /// sensor. + /// + /// \param[in] waitForReply Indicates if the method should wait for a + /// response from the sensor. + void restoreFactorySettings(bool waitForReply = true); + + /// \brief Issues a Reset command to the VectorNav sensor. + /// + /// \param[in] waitForReply Indicates if the method should wait for a + /// response from the sensor. + void reset(bool waitForReply = true); + + /// \brief Issues a Firmware Update command to the VectorNav sensor. + /// + /// \param[in] waitForReply Indicates if the method should wait for a + /// response from the sensor. + void firmwareUpdateMode(bool waitForReply = true); + + /// \brief Write a firmware update record to the bootloader + /// + /// \param[in] record The firmware update record to write to the bootloader + /// \param[in] waitForReply Indicates if the method should wait for a + /// response from the sensor. + void writeFirmwareUpdateRecord(const std::string & record, bool waitForReply = true); + + /// \brief Calibrate the bootloader with current baudrate. + /// + /// return string - Bootloader Version Number + std::string calibrateBootloader(); + + /// \brief Issues a change baudrate to the VectorNav sensor and then + /// reconnects the attached serial port at the new baudrate. + /// + /// \param[in] baudrate The new sensor baudrate. + void changeBaudRate(uint32_t baudrate); + + /// \brief This method will connect to the specified serial port, query the + /// attached sensor, and determine the type of device. + /// + /// \param[in] portName The COM port name to connect to. + /// \param[in] baudrate The baudrate to connect at. + //static void determineDeviceFamily(std::string portName, uint32_t baudrate); + + /// \brief This method will query the attached sensor and determine the + /// device family it belongs to. + /// + /// \return The determined device family. + Family determineDeviceFamily(); + + /// \brief This method determines which device family a VectorNav sensor + /// belongs to based on the provided model number. + /// + /// \return The determined device family. + static Family determineDeviceFamily(std::string modelNumber); + + /// \defgroup vnSensorEvents VnSensor Events + /// \brief This group of methods allow registering/unregistering for events + /// of the VnSensor. + /// + /// \{ + + /// \brief Registers a callback method for notification when raw data is + /// received. + /// + /// \param[in] userData Pointer to user data, which will be provided to the + /// callback method. + /// \param[in] handler The callback method. + void registerRawDataReceivedHandler(void * userData, RawDataReceivedHandler handler); + +#if PL150 +//Event event +#else +#if PYTHON -private: - struct Impl; - Impl *_pi; + PyObject * registerRawDataReceivedHandler(PyObject * callable); - // For Firmware Update Files - FILE* firmwareUpdateFile; - bool bootloaderFiltering; +#endif +#endif + /// \brief Unregisters the registered callback method. + void unregisterRawDataReceivedHandler(); + + /// \brief Registers a callback method for notification when a new possible + /// packet is found. + /// + /// \param[in] userData Pointer to user data, which will be provided to the + /// callback method. + /// \param[in] handler The callback method. + void registerPossiblePacketFoundHandler(void * userData, PossiblePacketFoundHandler handler); + + /// \brief Unregisters the registered callback method. + void unregisterPossiblePacketFoundHandler(); + + /// \brief Registers a callback method for notification when a new + /// asynchronous data packet is received. + /// + /// \param[in] userData Pointer to user data, which will be provided to the + /// callback method. + /// \param[in] handler The callback method. + void registerAsyncPacketReceivedHandler(void * userData, AsyncPacketReceivedHandler handler); + +#if PL150 +//packet, index, timestamp +//Event eventAsyncPacketRecieved; +#if PYTHON + AsyncPacketReceivedEvent eventAsyncPacketReceived; +#endif +#else +#if PYTHON + PyObject * registerAsyncPacketReceivedHandler(PyObject * callable); +#endif +#endif + + /// \brief Unregisters the registered callback method. + void unregisterAsyncPacketReceivedHandler(); + + /// \brief Registers a callback method for notification when an error + /// packet is received. + /// + /// \param[in] userData Pointer to user data, which will be provided to the + /// callback method. + /// \param[in] handler The callback method. + void registerErrorPacketReceivedHandler(void * userData, ErrorPacketReceivedHandler handler); + + /// \brief Unregisters the registered callback method. + void unregisterErrorPacketReceivedHandler(); + + /// \} + + /// \defgroup registerAccessMethods Register Access Methods + /// \brief This group of methods provide access to read and write to the + /// sensor's registers. + /// + /// \{ + + /// \brief Reads the Binary Output 1 register. + /// + /// \return The register's values. + BinaryOutputRegister readBinaryOutput1(); + + /// \brief Writes to the Binary Output 1 register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeBinaryOutput1(BinaryOutputRegister & fields, bool waitForReply = true); + + /// \brief Reads the Binary Output 2 register. + /// + /// \return The register's values. + BinaryOutputRegister readBinaryOutput2(); + + /// \brief Writes to the Binary Output 2 register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeBinaryOutput2(BinaryOutputRegister & fields, bool waitForReply = true); + + /// \brief Reads the Binary Output 3 register. + /// + /// \return The register's values. + BinaryOutputRegister readBinaryOutput3(); + + /// \brief Writes to the Binary Output 3 register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeBinaryOutput3(BinaryOutputRegister & fields, bool waitForReply = true); + + /// \brief Reads the Serial Baud Rate register for the specified port. + /// + /// \param[in] port The port number to read from. + /// \return The register's values. + uint32_t readSerialBaudRate(uint8_t port); + + /// \brief Writes to the Serial Baud Rate register for the specified port. + /// + /// \param[in] baudrate The register's Baud Rate field. + /// \param[in] port The port number to write to. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeSerialBaudRate(const uint32_t & baudrate, uint8_t port, bool waitForReply = true); + + /// \brief Reads the Async Data Output Type register. + /// + /// \param[in] port The port number to read from. + /// \return The register's values. + protocol::uart::AsciiAsync readAsyncDataOutputType(uint8_t port); + + /// \brief Writes to the Async Data Output Type register. + /// + /// \param[in] ador The register's ADOR field. + /// \param[in] port The port number to write to. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeAsyncDataOutputType( + protocol::uart::AsciiAsync ador, uint8_t port, bool waitForReply = true); + + /// \brief Reads the Async Data Output Frequency register. + /// + /// \param[in] port The port number to read from. + /// \return The register's values. + uint32_t readAsyncDataOutputFrequency(uint8_t port); + + /// \brief Writes to the Async Data Output Frequency register. + /// + /// \param[in] adof The register's ADOF field. + /// \param[in] port The port number to write to. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeAsyncDataOutputFrequency(const uint32_t & adof, uint8_t port, bool waitForReply = true); + + /// \brief Reads the INS Basic Configuration register for a VN-200 sensor. + /// + /// \return The register's values. + InsBasicConfigurationRegisterVn200 readInsBasicConfigurationVn200(); + + /// \brief Writes to the INS Basic Configuration register for a VN-200 sensor. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeInsBasicConfigurationVn200( + InsBasicConfigurationRegisterVn200 & fields, bool waitForReply = true); + + /// \brief Writes to the INS Basic Configuration register for a VN-200 sensor. + /// + /// \param[in] scenario Value for the Scenario field. + /// \param[in] ahrsAiding Value for the AhrsAiding field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeInsBasicConfigurationVn200( + protocol::uart::Scenario scenario, const uint8_t & ahrsAiding, bool waitForReply = true); + + /// \brief Reads the INS Basic Configuration register for a VN-300 sensor. + /// + /// \return The register's values. + InsBasicConfigurationRegisterVn300 readInsBasicConfigurationVn300(); + + /// \brief Writes to the INS Basic Configuration register for a VN-300 sensor. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeInsBasicConfigurationVn300( + InsBasicConfigurationRegisterVn300 & fields, bool waitForReply = true); + + /// \brief Writes to the INS Basic Configuration register for a VN-300 sensor. + /// + /// \param[in] scenario Value for the Scenario field. + /// \param[in] ahrsAiding Value for the AhrsAiding field. + /// \param[in] estBaseline Value for the EstBaseline field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeInsBasicConfigurationVn300( + protocol::uart::Scenario scenario, const uint8_t & ahrsAiding, const uint8_t & estBaseline, + bool waitForReply = true); + + /// \brief Reads the User Tag register. + /// + /// \return The register's values. + std::string readUserTag(); + + /// \brief Writes to the User Tag register. + /// + /// \param[in] tag The register's Tag field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeUserTag(const std::string & tag, bool waitForReply = true); + + /// \brief Reads the Model Number register. + /// + /// \return The register's values. + std::string readModelNumber(); + + /// \brief Reads the Hardware Revision register. + /// + /// \return The register's values. + uint32_t readHardwareRevision(); + + /// \brief Reads the Serial Number register. + /// + /// \return The register's values. + uint32_t readSerialNumber(); + + /// \brief Reads the Firmware Version register. + /// + /// \return The register's values. + std::string readFirmwareVersion(); + + /// \brief Reads the Serial Baud Rate register. + /// + /// \return The register's values. + uint32_t readSerialBaudRate(); + + /// \brief Writes to the Serial Baud Rate register. + /// + /// \param[in] baudrate The register's Baud Rate field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeSerialBaudRate(const uint32_t & baudrate, bool waitForReply = true); + + /// \brief Reads the Async Data Output Type register. + /// + /// \return The register's values. + protocol::uart::AsciiAsync readAsyncDataOutputType(); + + /// \brief Writes to the Async Data Output Type register. + /// + /// \param[in] ador The register's ADOR field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeAsyncDataOutputType(protocol::uart::AsciiAsync ador, bool waitForReply = true); + + /// \brief Reads the Async Data Output Frequency register. + /// + /// \return The register's values. + uint32_t readAsyncDataOutputFrequency(); + + /// \brief Writes to the Async Data Output Frequency register. + /// + /// \param[in] adof The register's ADOF field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeAsyncDataOutputFrequency(const uint32_t & adof, bool waitForReply = true); + + /// \brief Reads the Yaw Pitch Roll register. + /// + /// \return The register's values. + vn::math::vec3f readYawPitchRoll(); + + /// \brief Reads the Attitude Quaternion register. + /// + /// \return The register's values. + vn::math::vec4f readAttitudeQuaternion(); + + /// \brief Reads the Quaternion, Magnetic, Acceleration and Angular Rates register. + /// + /// \return The register's values. + QuaternionMagneticAccelerationAndAngularRatesRegister + readQuaternionMagneticAccelerationAndAngularRates(); + + /// \brief Reads the Magnetic Measurements register. + /// + /// \return The register's values. + vn::math::vec3f readMagneticMeasurements(); + + /// \brief Reads the Acceleration Measurements register. + /// + /// \return The register's values. + vn::math::vec3f readAccelerationMeasurements(); + + /// \brief Reads the Angular Rate Measurements register. + /// + /// \return The register's values. + vn::math::vec3f readAngularRateMeasurements(); + + /// \brief Reads the Magnetic, Acceleration and Angular Rates register. + /// + /// \return The register's values. + MagneticAccelerationAndAngularRatesRegister readMagneticAccelerationAndAngularRates(); + + /// \brief Reads the Magnetic and Gravity Reference Vectors register. + /// + /// \return The register's values. + MagneticAndGravityReferenceVectorsRegister readMagneticAndGravityReferenceVectors(); + + /// \brief Writes to the Magnetic and Gravity Reference Vectors register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeMagneticAndGravityReferenceVectors( + MagneticAndGravityReferenceVectorsRegister & fields, bool waitForReply = true); + + /// \brief Writes to the Magnetic and Gravity Reference Vectors register. + /// + /// \param[in] magRef Value for the MagRef field. + /// \param[in] accRef Value for the AccRef field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeMagneticAndGravityReferenceVectors( + const vn::math::vec3f & magRef, const vn::math::vec3f & accRef, bool waitForReply = true); + + /// \brief Reads the Filter Measurements Variance Parameters register. + /// + /// \return The register's values. + FilterMeasurementsVarianceParametersRegister readFilterMeasurementsVarianceParameters(); + + /// \brief Writes to the Filter Measurements Variance Parameters register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeFilterMeasurementsVarianceParameters( + FilterMeasurementsVarianceParametersRegister & fields, bool waitForReply = true); + + /// \brief Writes to the Filter Measurements Variance Parameters register. + /// + /// \param[in] angularWalkVariance Value for the Angular Walk Variance field. + /// \param[in] angularRateVariance Value for the Angular Rate Variance field. + /// \param[in] magneticVariance Value for the Magnetic Variance field. + /// \param[in] accelerationVariance Value for the Acceleration Variance field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeFilterMeasurementsVarianceParameters( + const float & angularWalkVariance, const vn::math::vec3f & angularRateVariance, + const vn::math::vec3f & magneticVariance, const vn::math::vec3f & accelerationVariance, + bool waitForReply = true); + + /// \brief Reads the Magnetometer Compensation register. + /// + /// \return The register's values. + MagnetometerCompensationRegister readMagnetometerCompensation(); + + /// \brief Writes to the Magnetometer Compensation register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeMagnetometerCompensation( + MagnetometerCompensationRegister & fields, bool waitForReply = true); + + /// \brief Writes to the Magnetometer Compensation register. + /// + /// \param[in] c Value for the C field. + /// \param[in] b Value for the B field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeMagnetometerCompensation( + const vn::math::mat3f & c, const vn::math::vec3f & b, bool waitForReply = true); + + /// \brief Reads the Filter Active Tuning Parameters register. + /// + /// \return The register's values. + FilterActiveTuningParametersRegister readFilterActiveTuningParameters(); + + /// \brief Writes to the Filter Active Tuning Parameters register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeFilterActiveTuningParameters( + FilterActiveTuningParametersRegister & fields, bool waitForReply = true); + + /// \brief Writes to the Filter Active Tuning Parameters register. + /// + /// \param[in] magneticDisturbanceGain Value for the Magnetic Disturbance Gain field. + /// \param[in] accelerationDisturbanceGain Value for the Acceleration Disturbance Gain field. + /// \param[in] magneticDisturbanceMemory Value for the Magnetic Disturbance Memory field. + /// \param[in] accelerationDisturbanceMemory Value for the Acceleration Disturbance Memory field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeFilterActiveTuningParameters( + const float & magneticDisturbanceGain, const float & accelerationDisturbanceGain, + const float & magneticDisturbanceMemory, const float & accelerationDisturbanceMemory, + bool waitForReply = true); + + /// \brief Reads the Acceleration Compensation register. + /// + /// \return The register's values. + AccelerationCompensationRegister readAccelerationCompensation(); + + /// \brief Writes to the Acceleration Compensation register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeAccelerationCompensation( + AccelerationCompensationRegister & fields, bool waitForReply = true); + + /// \brief Writes to the Acceleration Compensation register. + /// + /// \param[in] c Value for the C field. + /// \param[in] b Value for the B field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeAccelerationCompensation( + const vn::math::mat3f & c, const vn::math::vec3f & b, bool waitForReply = true); + + /// \brief Reads the Reference Frame Rotation register. + /// + /// \return The register's values. + vn::math::mat3f readReferenceFrameRotation(); + + /// \brief Writes to the Reference Frame Rotation register. + /// + /// \param[in] c The register's C field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeReferenceFrameRotation(const vn::math::mat3f & c, bool waitForReply = true); + + /// \brief Reads the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register. + /// + /// \return The register's values. + YawPitchRollMagneticAccelerationAndAngularRatesRegister + readYawPitchRollMagneticAccelerationAndAngularRates(); + + /// \brief Reads the Communication Protocol Control register. + /// + /// \return The register's values. + CommunicationProtocolControlRegister readCommunicationProtocolControl(); + + /// \brief Writes to the Communication Protocol Control register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeCommunicationProtocolControl( + CommunicationProtocolControlRegister & fields, bool waitForReply = true); + + /// \brief Writes to the Communication Protocol Control register. + /// + /// \param[in] serialCount Value for the SerialCount field. + /// \param[in] serialStatus Value for the SerialStatus field. + /// \param[in] spiCount Value for the SPICount field. + /// \param[in] spiStatus Value for the SPIStatus field. + /// \param[in] serialChecksum Value for the SerialChecksum field. + /// \param[in] spiChecksum Value for the SPIChecksum field. + /// \param[in] errorMode Value for the ErrorMode field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeCommunicationProtocolControl( + protocol::uart::CountMode serialCount, protocol::uart::StatusMode serialStatus, + protocol::uart::CountMode spiCount, protocol::uart::StatusMode spiStatus, + protocol::uart::ChecksumMode serialChecksum, protocol::uart::ChecksumMode spiChecksum, + protocol::uart::ErrorMode errorMode, bool waitForReply = true); + + /// \brief Reads the Synchronization Control register. + /// + /// \return The register's values. + SynchronizationControlRegister readSynchronizationControl(); + + /// \brief Writes to the Synchronization Control register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeSynchronizationControl( + SynchronizationControlRegister & fields, bool waitForReply = true); + + /// \brief Writes to the Synchronization Control register. + /// + /// \param[in] syncInMode Value for the SyncInMode field. + /// \param[in] syncInEdge Value for the SyncInEdge field. + /// \param[in] syncInSkipFactor Value for the SyncInSkipFactor field. + /// \param[in] syncOutMode Value for the SyncOutMode field. + /// \param[in] syncOutPolarity Value for the SyncOutPolarity field. + /// \param[in] syncOutSkipFactor Value for the SyncOutSkipFactor field. + /// \param[in] syncOutPulseWidth Value for the SyncOutPulseWidth field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeSynchronizationControl( + protocol::uart::SyncInMode syncInMode, protocol::uart::SyncInEdge syncInEdge, + const uint16_t & syncInSkipFactor, protocol::uart::SyncOutMode syncOutMode, + protocol::uart::SyncOutPolarity syncOutPolarity, const uint16_t & syncOutSkipFactor, + const uint32_t & syncOutPulseWidth, bool waitForReply = true); + + /// \brief Reads the Synchronization Status register. + /// + /// \return The register's values. + SynchronizationStatusRegister readSynchronizationStatus(); + + /// \brief Writes to the Synchronization Status register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeSynchronizationStatus(SynchronizationStatusRegister & fields, bool waitForReply = true); + + /// \brief Writes to the Synchronization Status register. + /// + /// \param[in] syncInCount Value for the SyncInCount field. + /// \param[in] syncInTime Value for the SyncInTime field. + /// \param[in] syncOutCount Value for the SyncOutCount field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeSynchronizationStatus( + const uint32_t & syncInCount, const uint32_t & syncInTime, const uint32_t & syncOutCount, + bool waitForReply = true); + + /// \brief Reads the Filter Basic Control register. + /// + /// \return The register's values. + FilterBasicControlRegister readFilterBasicControl(); + + /// \brief Writes to the Filter Basic Control register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeFilterBasicControl(FilterBasicControlRegister & fields, bool waitForReply = true); + + /// \brief Writes to the Filter Basic Control register. + /// + /// \param[in] magMode Value for the MagMode field. + /// \param[in] extMagMode Value for the ExtMagMode field. + /// \param[in] extAccMode Value for the ExtAccMode field. + /// \param[in] extGyroMode Value for the ExtGyroMode field. + /// \param[in] gyroLimit Value for the GyroLimit field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeFilterBasicControl( + protocol::uart::MagneticMode magMode, protocol::uart::ExternalSensorMode extMagMode, + protocol::uart::ExternalSensorMode extAccMode, protocol::uart::ExternalSensorMode extGyroMode, + const vn::math::vec3f & gyroLimit, bool waitForReply = true); + + /// \brief Reads the Heave Configuration register. + /// + /// \return The register's values. + HeaveConfigurationRegister readHeaveConfiguration(); + + /// \brief Writes to the Heave Configuration register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeHeaveConfiguration(HeaveConfigurationRegister & fields, bool waitForReply = true); + + /// \brief Writes to the Heave Configuration register. + /// + /// \param[in] initialWavePeriod Value for the initialWavePeriod field. + /// \param[in] initialWaveAmplitude Value for the initialWaveAmplitude field. + /// \param[in] maxWavePeriod Value for the maxWavePeriod field. + /// \param[in] minWaveAmplitude Value for the minWaveAmplitude field. + /// \param[in] delayedHeaveCutoffFreq Value for the delayedHeaveCutoffFreq field. + /// \param[in] heaveCutoffFreq Value for the heaveCutoffFreq field. + /// \param[in] heaveRateCutoffFreq Value for the heaveRateCutoffFreq field. + void writeHeaveConfiguration( + const float & initialWavePeriod, const float & initialWaveAmplitude, + const float & maxWavePeriod, const float & minWaveAmplitude, + const float & delayedHeaveCutoffFreq, const float & heaveCutoffFreq, + const float & heaveRateCutoffFreq, bool waitForReply = true); + + /// \brief Reads the VPE Basic Control register. + /// + /// \return The register's values. + VpeBasicControlRegister readVpeBasicControl(); + + /// \brief Writes to the VPE Basic Control register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeBasicControl(VpeBasicControlRegister & fields, bool waitForReply = true); + + /// \brief Writes to the VPE Basic Control register. + /// + /// \param[in] enable Value for the Enable field. + /// \param[in] headingMode Value for the HeadingMode field. + /// \param[in] filteringMode Value for the FilteringMode field. + /// \param[in] tuningMode Value for the TuningMode field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeBasicControl( + protocol::uart::VpeEnable enable, protocol::uart::HeadingMode headingMode, + protocol::uart::VpeMode filteringMode, protocol::uart::VpeMode tuningMode, + bool waitForReply = true); + + /// \brief Reads the VPE Magnetometer Basic Tuning register. + /// + /// \return The register's values. + VpeMagnetometerBasicTuningRegister readVpeMagnetometerBasicTuning(); + + /// \brief Writes to the VPE Magnetometer Basic Tuning register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeMagnetometerBasicTuning( + VpeMagnetometerBasicTuningRegister & fields, bool waitForReply = true); + + /// \brief Writes to the VPE Magnetometer Basic Tuning register. + /// + /// \param[in] baseTuning Value for the BaseTuning field. + /// \param[in] adaptiveTuning Value for the AdaptiveTuning field. + /// \param[in] adaptiveFiltering Value for the AdaptiveFiltering field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeMagnetometerBasicTuning( + const vn::math::vec3f & baseTuning, const vn::math::vec3f & adaptiveTuning, + const vn::math::vec3f & adaptiveFiltering, bool waitForReply = true); + + /// \brief Reads the VPE Magnetometer Advanced Tuning register. + /// + /// \return The register's values. + VpeMagnetometerAdvancedTuningRegister readVpeMagnetometerAdvancedTuning(); + + /// \brief Writes to the VPE Magnetometer Advanced Tuning register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeMagnetometerAdvancedTuning( + VpeMagnetometerAdvancedTuningRegister & fields, bool waitForReply = true); + + /// \brief Writes to the VPE Magnetometer Advanced Tuning register. + /// + /// \param[in] minFiltering Value for the MinFiltering field. + /// \param[in] maxFiltering Value for the MaxFiltering field. + /// \param[in] maxAdaptRate Value for the MaxAdaptRate field. + /// \param[in] disturbanceWindow Value for the DisturbanceWindow field. + /// \param[in] maxTuning Value for the MaxTuning field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeMagnetometerAdvancedTuning( + const vn::math::vec3f & minFiltering, const vn::math::vec3f & maxFiltering, + const float & maxAdaptRate, const float & disturbanceWindow, const float & maxTuning, + bool waitForReply = true); + + /// \brief Reads the VPE Accelerometer Basic Tuning register. + /// + /// \return The register's values. + VpeAccelerometerBasicTuningRegister readVpeAccelerometerBasicTuning(); + + /// \brief Writes to the VPE Accelerometer Basic Tuning register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeAccelerometerBasicTuning( + VpeAccelerometerBasicTuningRegister & fields, bool waitForReply = true); + + /// \brief Writes to the VPE Accelerometer Basic Tuning register. + /// + /// \param[in] baseTuning Value for the BaseTuning field. + /// \param[in] adaptiveTuning Value for the AdaptiveTuning field. + /// \param[in] adaptiveFiltering Value for the AdaptiveFiltering field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeAccelerometerBasicTuning( + const vn::math::vec3f & baseTuning, const vn::math::vec3f & adaptiveTuning, + const vn::math::vec3f & adaptiveFiltering, bool waitForReply = true); + + /// \brief Reads the VPE Accelerometer Advanced Tuning register. + /// + /// \return The register's values. + VpeAccelerometerAdvancedTuningRegister readVpeAccelerometerAdvancedTuning(); + + /// \brief Writes to the VPE Accelerometer Advanced Tuning register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeAccelerometerAdvancedTuning( + VpeAccelerometerAdvancedTuningRegister & fields, bool waitForReply = true); + + /// \brief Writes to the VPE Accelerometer Advanced Tuning register. + /// + /// \param[in] minFiltering Value for the MinFiltering field. + /// \param[in] maxFiltering Value for the MaxFiltering field. + /// \param[in] maxAdaptRate Value for the MaxAdaptRate field. + /// \param[in] disturbanceWindow Value for the DisturbanceWindow field. + /// \param[in] maxTuning Value for the MaxTuning field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeAccelerometerAdvancedTuning( + const vn::math::vec3f & minFiltering, const vn::math::vec3f & maxFiltering, + const float & maxAdaptRate, const float & disturbanceWindow, const float & maxTuning, + bool waitForReply = true); + + /// \brief Reads the VPE Gyro Basic Tuning register. + /// + /// \return The register's values. + VpeGyroBasicTuningRegister readVpeGyroBasicTuning(); + + /// \brief Writes to the VPE Gyro Basic Tuning register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeGyroBasicTuning(VpeGyroBasicTuningRegister & fields, bool waitForReply = true); + + /// \brief Writes to the VPE Gyro Basic Tuning register. + /// + /// \param[in] angularWalkVariance Value for the AngularWalkVariance field. + /// \param[in] baseTuning Value for the BaseTuning field. + /// \param[in] adaptiveTuning Value for the AdaptiveTuning field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeGyroBasicTuning( + const vn::math::vec3f & angularWalkVariance, const vn::math::vec3f & baseTuning, + const vn::math::vec3f & adaptiveTuning, bool waitForReply = true); + + /// \brief Reads the Filter Startup Gyro Bias register. + /// + /// \return The register's values. + vn::math::vec3f readFilterStartupGyroBias(); + + /// \brief Writes to the Filter Startup Gyro Bias register. + /// + /// \param[in] bias The register's Bias field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeFilterStartupGyroBias(const vn::math::vec3f & bias, bool waitForReply = true); + + /// \brief Reads the Magnetometer Calibration Control register. + /// + /// \return The register's values. + MagnetometerCalibrationControlRegister readMagnetometerCalibrationControl(); + + /// \brief Writes to the Magnetometer Calibration Control register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeMagnetometerCalibrationControl( + MagnetometerCalibrationControlRegister & fields, bool waitForReply = true); + + /// \brief Writes to the Magnetometer Calibration Control register. + /// + /// \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] waitForReply Indicates if the method should wait for a response from the sensor. + void writeMagnetometerCalibrationControl( + protocol::uart::HsiMode hsiMode, protocol::uart::HsiOutput hsiOutput, + const uint8_t & convergeRate, bool waitForReply = true); + + /// \brief Reads the Calculated Magnetometer Calibration register. + /// + /// \return The register's values. + CalculatedMagnetometerCalibrationRegister readCalculatedMagnetometerCalibration(); + + /// \brief Reads the Indoor Heading Mode Control register. + /// + /// \return The register's values. + float readIndoorHeadingModeControl(); + + /// \brief Writes to the Indoor Heading Mode Control register. + /// + /// \param[in] maxRateError The register's Max Rate Error field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeIndoorHeadingModeControl(const float & maxRateError, bool waitForReply = true); + + /// \brief Reads the Velocity Compensation Measurement register. + /// + /// \return The register's values. + vn::math::vec3f readVelocityCompensationMeasurement(); + + /// \brief Writes to the Velocity Compensation Measurement register. + /// + /// \param[in] velocity The register's Velocity field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVelocityCompensationMeasurement( + const vn::math::vec3f & velocity, bool waitForReply = true); + + /// \brief Reads the Velocity Compensation Control register. + /// + /// \return The register's values. + VelocityCompensationControlRegister readVelocityCompensationControl(); + + /// \brief Writes to the Velocity Compensation Control register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVelocityCompensationControl( + VelocityCompensationControlRegister & fields, bool waitForReply = true); + + /// \brief Writes to the Velocity Compensation Control register. + /// + /// \param[in] mode Value for the Mode field. + /// \param[in] velocityTuning Value for the VelocityTuning field. + /// \param[in] rateTuning Value for the RateTuning field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVelocityCompensationControl( + protocol::uart::VelocityCompensationMode mode, const float & velocityTuning, + const float & rateTuning, bool waitForReply = true); + + /// \brief Reads the Velocity Compensation Status register. + /// + /// \return The register's values. + VelocityCompensationStatusRegister readVelocityCompensationStatus(); + + /// \brief Reads the IMU Measurements register. + /// + /// \return The register's values. + ImuMeasurementsRegister readImuMeasurements(); + + /// \brief Reads the GPS Configuration register. + /// + /// \return The register's values. + GpsConfigurationRegister readGpsConfiguration(); + + /// \brief Writes to the GPS Configuration register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeGpsConfiguration(GpsConfigurationRegister & fields, bool waitForReply = true); + + /// \brief Writes to the GPS Configuration register (deprecated). + /// + /// \param[in] mode Value for the Mode field. + /// \param[in] ppsSource Value for the PpsSource field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeGpsConfiguration( + protocol::uart::GpsMode mode, protocol::uart::PpsSource ppsSource, bool waitForReply = true); + + /// \brief Writes to the GPS Configuration register. + /// + /// \param[in] mode Value for the Mode field. + /// \param[in] ppsSource Value for the PpsSource field. + /// \param[in] rate Value for the Rate field. + /// \param[in] antPow Value for the AntPow field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeGpsConfiguration( + protocol::uart::GpsMode mode, protocol::uart::PpsSource ppsSource, protocol::uart::GpsRate rate, + protocol::uart::AntPower antPow, bool waitForReply = true); + + /// \brief Reads the GPS Antenna Offset register. + /// + /// \return The register's values. + vn::math::vec3f readGpsAntennaOffset(); + + /// \brief Writes to the GPS Antenna Offset register. + /// + /// \param[in] position The register's Position field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeGpsAntennaOffset(const vn::math::vec3f & position, bool waitForReply = true); + + /// \brief Reads the GPS Solution - LLA register. + /// + /// \return The register's values. + GpsSolutionLlaRegister readGpsSolutionLla(); + + /// \brief Reads the GPS Solution - ECEF register. + /// + /// \return The register's values. + GpsSolutionEcefRegister readGpsSolutionEcef(); + + /// \brief Reads the INS Solution - LLA register. + /// + /// \return The register's values. + InsSolutionLlaRegister readInsSolutionLla(); + + /// \brief Reads the INS Solution - ECEF register. + /// + /// \return The register's values. + InsSolutionEcefRegister readInsSolutionEcef(); + + /// \brief Reads the INS Advanced Configuration register. + /// + /// \return The register's values. + InsAdvancedConfigurationRegister readInsAdvancedConfiguration(); + + /// \brief Writes to the INS Advanced Configuration register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeInsAdvancedConfiguration( + InsAdvancedConfigurationRegister & fields, bool waitForReply = true); + + /// \brief Writes to the INS Advanced Configuration register. + /// + /// \param[in] useMag Value for the UseMag field. + /// \param[in] usePres Value for the UsePres field. + /// \param[in] posAtt Value for the PosAtt field. + /// \param[in] velAtt Value for the VelAtt field. + /// \param[in] velBias Value for the VelBias field. + /// \param[in] useFoam Value for the UseFoam field. + /// \param[in] gpsCovType Value for the GPSCovType field. + /// \param[in] velCount Value for the VelCount field. + /// \param[in] velInit Value for the VelInit field. + /// \param[in] moveOrigin Value for the MoveOrigin field. + /// \param[in] gpsTimeout Value for the GPSTimeout field. + /// \param[in] deltaLimitPos Value for the DeltaLimitPos field. + /// \param[in] deltaLimitVel Value for the DeltaLimitVel field. + /// \param[in] minPosUncertainty Value for the MinPosUncertainty field. + /// \param[in] minVelUncertainty Value for the MinVelUncertainty field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeInsAdvancedConfiguration( + const uint8_t & useMag, const uint8_t & usePres, const uint8_t & posAtt, const uint8_t & velAtt, + const uint8_t & velBias, protocol::uart::FoamInit useFoam, const uint8_t & gpsCovType, + const uint8_t & velCount, const float & velInit, const float & moveOrigin, + const float & gpsTimeout, const float & deltaLimitPos, const float & deltaLimitVel, + const float & minPosUncertainty, const float & minVelUncertainty, bool waitForReply = true); + + /// \brief Reads the INS State - LLA register. + /// + /// \return The register's values. + InsStateLlaRegister readInsStateLla(); + + /// \brief Reads the INS State - ECEF register. + /// + /// \return The register's values. + InsStateEcefRegister readInsStateEcef(); + + /// \brief Reads the Startup Filter Bias Estimate register. + /// + /// \return The register's values. + StartupFilterBiasEstimateRegister readStartupFilterBiasEstimate(); + + /// \brief Writes to the Startup Filter Bias Estimate register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeStartupFilterBiasEstimate( + StartupFilterBiasEstimateRegister & fields, bool waitForReply = true); + + /// \brief Writes to the Startup Filter Bias Estimate register. + /// + /// \param[in] gyroBias Value for the GyroBias field. + /// \param[in] accelBias Value for the AccelBias field. + /// \param[in] pressureBias Value for the PressureBias field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeStartupFilterBiasEstimate( + const vn::math::vec3f & gyroBias, const vn::math::vec3f & accelBias, const float & pressureBias, + bool waitForReply = true); + + /// \brief Reads the Delta Theta and Delta Velocity register. + /// + /// \return The register's values. + DeltaThetaAndDeltaVelocityRegister readDeltaThetaAndDeltaVelocity(); + + /// \brief Reads the Delta Theta and Delta Velocity Configuration register. + /// + /// \return The register's values. + DeltaThetaAndDeltaVelocityConfigurationRegister readDeltaThetaAndDeltaVelocityConfiguration(); + + /// \brief Writes to the Delta Theta and Delta Velocity Configuration register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeDeltaThetaAndDeltaVelocityConfiguration( + DeltaThetaAndDeltaVelocityConfigurationRegister & fields, bool waitForReply = true); + + /// \brief Writes to the Delta Theta and Delta Velocity Configuration register (deprecated). + /// + /// \param[in] integrationFrame Value for the IntegrationFrame field. + /// \param[in] gyroCompensation Value for the GyroCompensation field. + /// \param[in] accelCompensation Value for the AccelCompensation field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeDeltaThetaAndDeltaVelocityConfiguration( + protocol::uart::IntegrationFrame integrationFrame, + protocol::uart::CompensationMode gyroCompensation, + protocol::uart::AccCompensationMode accelCompensation, bool waitForReply = true); + + /// \brief Writes to the Delta Theta and Delta Velocity Configuration register. + /// + /// \param[in] integrationFrame Value for the IntegrationFrame field. + /// \param[in] gyroCompensation Value for the GyroCompensation field. + /// \param[in] accelCompensation Value for the AccelCompensation field. + /// \param[in] earthRateCorrection Value for the EarthRateCorrection field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeDeltaThetaAndDeltaVelocityConfiguration( + protocol::uart::IntegrationFrame integrationFrame, + protocol::uart::CompensationMode gyroCompensation, + protocol::uart::AccCompensationMode accelCompensation, + protocol::uart::EarthRateCorrection earthRateCorrection, bool waitForReply = true); + + /// \brief Reads the Reference Vector Configuration register. + /// + /// \return The register's values. + ReferenceVectorConfigurationRegister readReferenceVectorConfiguration(); + + /// \brief Writes to the Reference Vector Configuration register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeReferenceVectorConfiguration( + ReferenceVectorConfigurationRegister & fields, bool waitForReply = true); + + /// \brief Writes to the Reference Vector Configuration register. + /// + /// \param[in] useMagModel Value for the UseMagModel field. + /// \param[in] useGravityModel Value for the UseGravityModel field. + /// \param[in] recalcThreshold Value for the RecalcThreshold field. + /// \param[in] year Value for the Year field. + /// \param[in] position Value for the Position field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeReferenceVectorConfiguration( + const uint8_t & useMagModel, const uint8_t & useGravityModel, const uint32_t & recalcThreshold, + const float & year, const vn::math::vec3d & position, bool waitForReply = true); + + /// \brief Reads the Gyro Compensation register. + /// + /// \return The register's values. + GyroCompensationRegister readGyroCompensation(); + + /// \brief Writes to the Gyro Compensation register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeGyroCompensation(GyroCompensationRegister & fields, bool waitForReply = true); + + /// \brief Writes to the Gyro Compensation register. + /// + /// \param[in] c Value for the C field. + /// \param[in] b Value for the B field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeGyroCompensation( + const vn::math::mat3f & c, const vn::math::vec3f & b, bool waitForReply = true); + + /// \brief Reads the IMU Filtering Configuration register. + /// + /// \return The register's values. + ImuFilteringConfigurationRegister readImuFilteringConfiguration(); + + /// \brief Writes to the IMU Filtering Configuration register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeImuFilteringConfiguration( + ImuFilteringConfigurationRegister & fields, bool waitForReply = true); + + /// \brief Writes to the IMU Filtering Configuration register. + /// + /// \param[in] magWindowSize Value for the MagWindowSize field. + /// \param[in] accelWindowSize Value for the AccelWindowSize field. + /// \param[in] gyroWindowSize Value for the GyroWindowSize field. + /// \param[in] tempWindowSize Value for the TempWindowSize field. + /// \param[in] presWindowSize Value for the PresWindowSize field. + /// \param[in] magFilterMode Value for the MagFilterMode field. + /// \param[in] accelFilterMode Value for the AccelFilterMode field. + /// \param[in] gyroFilterMode Value for the GyroFilterMode field. + /// \param[in] tempFilterMode Value for the TempFilterMode field. + /// \param[in] presFilterMode Value for the PresFilterMode field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeImuFilteringConfiguration( + const uint16_t & magWindowSize, const uint16_t & accelWindowSize, + const uint16_t & gyroWindowSize, const uint16_t & tempWindowSize, + const uint16_t & presWindowSize, protocol::uart::FilterMode magFilterMode, + protocol::uart::FilterMode accelFilterMode, protocol::uart::FilterMode gyroFilterMode, + protocol::uart::FilterMode tempFilterMode, protocol::uart::FilterMode presFilterMode, + bool waitForReply = true); + + /// \brief Reads the GPS Compass Baseline register. + /// + /// \return The register's values. + GpsCompassBaselineRegister readGpsCompassBaseline(); + + /// \brief Writes to the GPS Compass Baseline register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeGpsCompassBaseline(GpsCompassBaselineRegister & fields, bool waitForReply = true); + + /// \brief Writes to the GPS Compass Baseline register. + /// + /// \param[in] position Value for the Position field. + /// \param[in] uncertainty Value for the Uncertainty field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeGpsCompassBaseline( + const vn::math::vec3f & position, const vn::math::vec3f & uncertainty, + bool waitForReply = true); + + /// \brief Reads the GPS Compass Estimated Baseline register. + /// + /// \return The register's values. + GpsCompassEstimatedBaselineRegister readGpsCompassEstimatedBaseline(); + + /// \brief Reads the IMU Rate Configuration register. + /// + /// \return The register's values. + ImuRateConfigurationRegister readImuRateConfiguration(); + + /// \brief Writes to the IMU Rate Configuration register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeImuRateConfiguration(ImuRateConfigurationRegister & fields, bool waitForReply = true); + + /// \brief Writes to the IMU Rate Configuration register. + /// + /// \param[in] imuRate Value for the imuRate field. + /// \param[in] navDivisor Value for the NavDivisor field. + /// \param[in] filterTargetRate Value for the filterTargetRate field. + /// \param[in] filterMinRate Value for the filterMinRate field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeImuRateConfiguration( + const uint16_t & imuRate, const uint16_t & navDivisor, const float & filterTargetRate, + const float & filterMinRate, bool waitForReply = true); + + /// \brief Reads the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register. + /// + /// \return The register's values. + YawPitchRollTrueBodyAccelerationAndAngularRatesRegister + readYawPitchRollTrueBodyAccelerationAndAngularRates(); + + /// \brief Reads the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register. + /// + /// \return The register's values. + YawPitchRollTrueInertialAccelerationAndAngularRatesRegister + readYawPitchRollTrueInertialAccelerationAndAngularRates(); + + /// \brief Upgrade the connected sensor with the supplied firmware file + /// + /// \param[in] processor The target processor to switch to. + /// \param[in] model The model of sensor. + /// \param[in] firmware The current firmware version of sensor. + void switchProcessors(VnProcessorType processor, std::string model, std::string firmware); + + /// \brief Upgrade the connected sensor with the supplied firmware file + /// + /// \param]in] portName The baud rate used to update the firmware. (Can be different than the current baud rate) + /// \param[in] filename The path to the VNX firmware file for the sensor. + void firmwareUpdate(int baudRate, std::string fileName); + + /// \brief Upgrade the connected sensor with the supplied firmware file + /// + /// \param]in] portName The baud rate used to update the firmware. (Can be different than the current baud rate) + void setFirmwareUpdateBaudRate(int baudRate); + + /// \brief Get the next Firmware Update Record + /// + /// \return The next record in the Firmware Update file, else an empty string + std::string getNextFirwareUpdateRecord(); + + /// \brief Open the firmware update file + void openFirmwareUpdateFile(std::string filename); + + /// \brief Close the firmware update file + void closeFirmwareUpdateFile(); + + /// \} + +#ifdef PYTHON_WRAPPERS + +#endif + +#if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 + + void stopRequest(); + bool threadStopped(); + void unregisterListners(); + void shutdownRequest(); + void goRequest(); + +#endif + +private: + struct Impl; + Impl * _pi; + + // For Firmware Update Files + FILE * firmwareUpdateFile; + bool bootloaderFiltering; }; -} -} +} // namespace sensors +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/serialport.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/serialport.h index 201b5286..a847d783 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/serialport.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/serialport.h @@ -2,24 +2,26 @@ #define _VN_XPLAT_SERIALPORT_H_ #if _MSC_VER && _WIN32 - #pragma comment(lib, "setupapi.lib") +#pragma comment(lib, "setupapi.lib") #endif +#include #include #include -#include #if PYTHON - #include "boostpython.h" +#include "boostpython.h" #endif +#include "export.h" #include "int.h" -#include "port.h" #include "nocopy.h" -#include "export.h" +#include "port.h" -namespace vn { -namespace xplat { +namespace vn +{ +namespace xplat +{ /// \brief Represents a cross-platform serial port. /// @@ -33,122 +35,112 @@ namespace xplat { /// data. class vn_proglib_DLLEXPORT SerialPort : public IPort, util::NoCopy { + // Types ////////////////////////////////////////////////////////////////// - // Types ////////////////////////////////////////////////////////////////// - public: - - enum StopBits - { - ONE_STOP_BIT, - TWO_STOP_BITS - }; + enum StopBits { ONE_STOP_BIT, TWO_STOP_BITS }; - // Constructors /////////////////////////////////////////////////////////// + // Constructors /////////////////////////////////////////////////////////// public: + /// \brief Creates a new \ref SerialPort with the provided connection + /// parameters. + /// + /// \param[in] portName The name of the serial port. + /// \param[in] baudrate The baudrate to open the serial port at. + SerialPort(const std::string & portName, uint32_t baudrate); - /// \brief Creates a new \ref SerialPort with the provided connection - /// parameters. - /// - /// \param[in] portName The name of the serial port. - /// \param[in] baudrate The baudrate to open the serial port at. - SerialPort(const std::string& portName, uint32_t baudrate); + ~SerialPort(); - ~SerialPort(); - - // Public Methods ///////////////////////////////////////////////////////// + // Public Methods ///////////////////////////////////////////////////////// public: + /// \brief Returns a list of the names of all the available serial ports on + /// the system. + /// + /// \return The list of available serial port names. + static std::vector getPortNames(); - /// \brief Returns a list of the names of all the available serial ports on - /// the system. - /// - /// \return The list of available serial port names. - static std::vector getPortNames(); - - virtual void open(); - - virtual void close(); - - virtual bool isOpen(); - - virtual void write(const char data[], size_t length); - - virtual void read(char dataBuffer[], size_t numOfBytesToRead, size_t &numOfBytesActuallyRead); - - virtual void registerDataReceivedHandler(void* userData, DataReceivedHandler handler); - - virtual void unregisterDataReceivedHandler(); - - /// \brief Returns the baudrate connected at. - /// - /// \return The connected baudrate. - uint32_t baudrate(); - - /// \brief Returns the port connected to. - /// - /// \return The port name. - std::string port(); - - /// \brief Changes the connected baudrate of the port. - /// - /// \param[in] br The baudrate to change the port to. - void changeBaudrate(uint32_t br); - - /// \brief Returns the stop bit configuration. - /// - /// \return The current stop bit configuration. - StopBits stopBits(); - - /// \brief Sets the stop bit configuration. - /// - /// \param[in] stopBits The stop bit configuration. - void setStopBits(StopBits stopBits); - - /// \brief Indicates if the platforms supports event notifications. - - /// \brief Returns the number of dropped sections of received data. - /// - /// \return The number of sections of dropped data sections. Note this is - /// not indicative of the total number of dropped bytes. - size_t NumberOfReceiveDataDroppedSections(); - - /// \brief With regard to optimizing COM ports provided by FTDI drivers, this - /// method will check if the COM port has been optimized. - /// - /// \param[in] portName The COM port name to check. - /// \return true if the COM port is optimized; otherwise false. - static bool determineIfPortIsOptimized(std::string portName); - - /// \brief This will perform optimization of FTDI USB serial ports. - /// - /// If calling this method on Windows, the process must have administrator - /// privileges to write settings to the registry. Otherwise an - /// - /// \param[in] portName The FTDI USB Serial Port to optimize. - static void optimizePort(std::string portName); - - #if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 - - virtual void stopThread(); - virtual void resumeThread(); - virtual bool threadStopped(); - - #endif - - // Private Members //////////////////////////////////////////////////////// - -private: + virtual void open(); + + virtual void close(); + + virtual bool isOpen(); + + virtual void write(const char data[], size_t length); + + virtual void read(char dataBuffer[], size_t numOfBytesToRead, size_t & numOfBytesActuallyRead); + + virtual void registerDataReceivedHandler(void * userData, DataReceivedHandler handler); - // Contains internal data, mainly stuff that is required for cross-platform - // support. - struct Impl; - Impl *_pi; + virtual void unregisterDataReceivedHandler(); + /// \brief Returns the baudrate connected at. + /// + /// \return The connected baudrate. + uint32_t baudrate(); + + /// \brief Returns the port connected to. + /// + /// \return The port name. + std::string port(); + + /// \brief Changes the connected baudrate of the port. + /// + /// \param[in] br The baudrate to change the port to. + void changeBaudrate(uint32_t br); + + /// \brief Returns the stop bit configuration. + /// + /// \return The current stop bit configuration. + StopBits stopBits(); + + /// \brief Sets the stop bit configuration. + /// + /// \param[in] stopBits The stop bit configuration. + void setStopBits(StopBits stopBits); + + /// \brief Indicates if the platforms supports event notifications. + + /// \brief Returns the number of dropped sections of received data. + /// + /// \return The number of sections of dropped data sections. Note this is + /// not indicative of the total number of dropped bytes. + size_t NumberOfReceiveDataDroppedSections(); + + /// \brief With regard to optimizing COM ports provided by FTDI drivers, this + /// method will check if the COM port has been optimized. + /// + /// \param[in] portName The COM port name to check. + /// \return true if the COM port is optimized; otherwise false. + static bool determineIfPortIsOptimized(std::string portName); + + /// \brief This will perform optimization of FTDI USB serial ports. + /// + /// If calling this method on Windows, the process must have administrator + /// privileges to write settings to the registry. Otherwise an + /// + /// \param[in] portName The FTDI USB Serial Port to optimize. + static void optimizePort(std::string portName); + +#if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 + + virtual void stopThread(); + virtual void resumeThread(); + virtual bool threadStopped(); + +#endif + + // Private Members //////////////////////////////////////////////////////// + +private: + // Contains internal data, mainly stuff that is required for cross-platform + // support. + struct Impl; + Impl * _pi; }; -} -} +} // namespace xplat +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/signal.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/signal.h index 5c30ff66..a439b309 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/signal.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/signal.h @@ -1,91 +1,84 @@ #ifndef _VNXPLAT_SIGNAL_H_ #define _VNXPLAT_SIGNAL_H_ -namespace vn { -namespace xplat { +namespace vn +{ +namespace xplat +{ /// \brief Provides access to system signals. class Signal { - // Types ////////////////////////////////////////////////////////////////// + // Types ////////////////////////////////////////////////////////////////// public: - - /// \brief The available signals. - enum SignalType - { - /// Unknown type of signal. - UNKNOWN, - - /// User pressed Ctrl-C. - CTRL_C - }; - - /// \brief Typedef for a function that can handle signal notifications. - /// - /// \param[in] signal The signal type received. - /// \return Indicates if the signal was handled or not. - typedef bool (*HandleSignalFunc)(Signal signal); - - /// \brief Allows for other objects to listen for signal events. - class Observer - { - public: - virtual ~Observer() { } - - /// \brief If an observer is registered via - /// \ref RegisterSignalObserver, whenever a signal is received, - /// this method will be called to alert the observer. - /// - /// \param[in] signal The signal that was raised. - /// \return Indicates if the signal was handled or not. - virtual bool XSignalHandleSingle(SignalType signal) = 0; - - protected: - Observer() { } - }; - - // Public Methods ///////////////////////////////////////////////////////// + /// \brief The available signals. + enum SignalType { + /// Unknown type of signal. + UNKNOWN, + + /// User pressed Ctrl-C. + CTRL_C + }; + + /// \brief Typedef for a function that can handle signal notifications. + /// + /// \param[in] signal The signal type received. + /// \return Indicates if the signal was handled or not. + typedef bool (*HandleSignalFunc)(Signal signal); + + /// \brief Allows for other objects to listen for signal events. + class Observer + { + public: + virtual ~Observer() {} + + /// \brief If an observer is registered via + /// \ref RegisterSignalObserver, whenever a signal is received, + /// this method will be called to alert the observer. + /// + /// \param[in] signal The signal that was raised. + /// \return Indicates if the signal was handled or not. + virtual bool XSignalHandleSingle(SignalType signal) = 0; + + protected: + Observer() {} + }; + + // Public Methods ///////////////////////////////////////////////////////// public: - - /// \brief Allows registering to receive notifications of system signals. - /// - /// \param[in] handleFunc Function to call when signals are received. - static void RegisterForSignalNotifications( - HandleSignalFunc handleFunc); - - /// \brief Allows unregistering from receiving signal notifications. - /// - /// \param[in] handleFunc The function to unregister. - static void UnregisterForSignalNotifications( - HandleSignalFunc handleFunc); - - /// \brief Allows registering an observer for notification when a signal is - /// received. - /// - /// \param[in] observer The observer to register. - static void RegisterSignalObserver( - Observer* observer); - - /// \brief Allows unregistering of an observer from being notified when a - /// signal is received. - /// - /// \param[in] observer The observer to unregister. - static void UnregisterSignalObserver( - Observer* observer); - - // Private Members //////////////////////////////////////////////////////// + /// \brief Allows registering to receive notifications of system signals. + /// + /// \param[in] handleFunc Function to call when signals are received. + static void RegisterForSignalNotifications(HandleSignalFunc handleFunc); + + /// \brief Allows unregistering from receiving signal notifications. + /// + /// \param[in] handleFunc The function to unregister. + static void UnregisterForSignalNotifications(HandleSignalFunc handleFunc); + + /// \brief Allows registering an observer for notification when a signal is + /// received. + /// + /// \param[in] observer The observer to register. + static void RegisterSignalObserver(Observer * observer); + + /// \brief Allows unregistering of an observer from being notified when a + /// signal is received. + /// + /// \param[in] observer The observer to unregister. + static void UnregisterSignalObserver(Observer * observer); + + // Private Members //////////////////////////////////////////////////////// private: - - // Contains internal data, mainly stuff that is required for cross-platform - // support. - struct Impl; - + // Contains internal data, mainly stuff that is required for cross-platform + // support. + struct Impl; }; -} -} +} // namespace xplat +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/thread.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/thread.h index 23946c2e..4119d949 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/thread.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/thread.h @@ -6,8 +6,8 @@ #ifndef _VNXPLAT_THREAD_H_ #define _VNXPLAT_THREAD_H_ -#include "int.h" #include "export.h" +#include "int.h" #include "nocopy.h" #if _WIN32 @@ -19,90 +19,85 @@ #error "Unknown System" #endif -namespace vn { -namespace xplat { +namespace vn +{ +namespace xplat +{ /// \brief Represents a cross-platform thread. class vn_proglib_DLLEXPORT Thread : private util::NoCopy { - - // Types ////////////////////////////////////////////////////////////////// + // Types ////////////////////////////////////////////////////////////////// public: + /// \brief Represents a start routine for a thread which will have data + /// passed to it. + typedef void (*ThreadStartRoutine)(void *); - /// \brief Represents a start routine for a thread which will have data - /// passed to it. - typedef void (*ThreadStartRoutine)(void*); - - // Constructors /////////////////////////////////////////////////////////// + // Constructors /////////////////////////////////////////////////////////// public: - - ~Thread(); + ~Thread(); private: + /// \brief Creates a new Thread object. + /// + /// \param[in] startRoutine The routine to call when the thread is started. + explicit Thread(ThreadStartRoutine startRoutine); - /// \brief Creates a new Thread object. - /// - /// \param[in] startRoutine The routine to call when the thread is started. - explicit Thread(ThreadStartRoutine startRoutine); - - // Public Methods ///////////////////////////////////////////////////////// + // Public Methods ///////////////////////////////////////////////////////// public: - - /// \brief Starts a new thread immediately which calls the provided start - /// routine and passes the routine data to the new thread. - /// - /// \param[in] startRoutine The routine to be called when the new thread is - /// started. - /// \param[in] routineData Pointer to data that will be passed to the new - /// thread via its start routine. - /// \return A Thread object representing the newly started thread. - /// User must delete the returned pointer when finished. - static Thread* startNew(ThreadStartRoutine startRoutine, void* routineData); - - /// \brief Starts the thread. - /// - /// \param[in] routineData Pointer to the routine data which the new thread - /// have access to. - void start(void* routineData); - - /// \brief Blocks the calling thread until this thread finishes. - void join(); - - /// \brief Causes the thread to sleep the specified number of seconds. - /// - /// \param[in] numOfSecsToSleep The number of seconds to sleep. - static void sleepSec(uint32_t numOfSecsToSleep); - - /// \brief Causes the thread to sleep the specified number of milliseconds. - /// - /// \param[in] numOfMsToSleep The number of milliseconds to sleep. - static void sleepMs(uint32_t numOfMsToSleep); - - /// \brief Causes the thread to sleep the specified number of microseconds. - /// - /// \param[in] numOfUsToSleep The number of microseconds to sleep. - static void sleepUs(uint32_t numOfUsToSleep); - - /// \brief Causes the thread to sleep the specified number of nanoseconds. - /// - /// \param[in] numOfNsToSleep The number of nanoseconds to sleep. - static void sleepNs(uint32_t numOfNsToSleep); - - // Private Members //////////////////////////////////////////////////////// + /// \brief Starts a new thread immediately which calls the provided start + /// routine and passes the routine data to the new thread. + /// + /// \param[in] startRoutine The routine to be called when the new thread is + /// started. + /// \param[in] routineData Pointer to data that will be passed to the new + /// thread via its start routine. + /// \return A Thread object representing the newly started thread. + /// User must delete the returned pointer when finished. + static Thread * startNew(ThreadStartRoutine startRoutine, void * routineData); + + /// \brief Starts the thread. + /// + /// \param[in] routineData Pointer to the routine data which the new thread + /// have access to. + void start(void * routineData); + + /// \brief Blocks the calling thread until this thread finishes. + void join(); + + /// \brief Causes the thread to sleep the specified number of seconds. + /// + /// \param[in] numOfSecsToSleep The number of seconds to sleep. + static void sleepSec(uint32_t numOfSecsToSleep); + + /// \brief Causes the thread to sleep the specified number of milliseconds. + /// + /// \param[in] numOfMsToSleep The number of milliseconds to sleep. + static void sleepMs(uint32_t numOfMsToSleep); + + /// \brief Causes the thread to sleep the specified number of microseconds. + /// + /// \param[in] numOfUsToSleep The number of microseconds to sleep. + static void sleepUs(uint32_t numOfUsToSleep); + + /// \brief Causes the thread to sleep the specified number of nanoseconds. + /// + /// \param[in] numOfNsToSleep The number of nanoseconds to sleep. + static void sleepNs(uint32_t numOfNsToSleep); + + // Private Members //////////////////////////////////////////////////////// private: - - // Contains internal data, mainly stuff that is required for cross-platform - // support. - struct Impl; - Impl *_pimpl; - + // Contains internal data, mainly stuff that is required for cross-platform + // support. + struct Impl; + Impl * _pimpl; }; -} -} +} // namespace xplat +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/types.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/types.h index 5d862b73..93f7e99f 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/types.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/types.h @@ -3,657 +3,620 @@ #include "int.h" -namespace vn { -namespace protocol { -namespace uart { - -enum ErrorDetectionMode +namespace vn +{ +namespace protocol { - ERRORDETECTIONMODE_NONE, ///< No error detection is used. - ERRORDETECTIONMODE_CHECKSUM, ///< 8-bit XOR checksum is used. - ERRORDETECTIONMODE_CRC ///< 16-bit CRC16-CCITT is used. +namespace uart +{ + +enum ErrorDetectionMode { + ERRORDETECTIONMODE_NONE, ///< No error detection is used. + ERRORDETECTIONMODE_CHECKSUM, ///< 8-bit XOR checksum is used. + ERRORDETECTIONMODE_CRC ///< 16-bit CRC16-CCITT is used. }; /// \brief Enumeration of the available asynchronous ASCII message types. -enum AsciiAsync -{ - VNOFF = 0, ///< Asynchronous output is turned off. - VNYPR = 1, ///< Asynchronous output type is Yaw, Pitch, Roll. - VNQTN = 2, ///< Asynchronous output type is Quaternion. - #ifdef INTERNAL - VNQTM = 3, ///< Asynchronous output type is Quaternion and Magnetic. - VNQTA = 4, ///< Asynchronous output type is Quaternion and Acceleration. - VNQTR = 5, ///< Asynchronous output type is Quaternion and Angular Rates. - VNQMA = 6, ///< Asynchronous output type is Quaternion, Magnetic and Acceleration. - VNQAR = 7, ///< Asynchronous output type is Quaternion, Acceleration and Angular Rates. - #endif - VNQMR = 8, ///< Asynchronous output type is Quaternion, Magnetic, Acceleration and Angular Rates. - #ifdef INTERNAL - VNDCM = 9, ///< Asynchronous output type is Directional Cosine Orientation Matrix. - #endif - VNMAG = 10, ///< Asynchronous output type is Magnetic Measurements. - VNACC = 11, ///< Asynchronous output type is Acceleration Measurements. - VNGYR = 12, ///< Asynchronous output type is Angular Rate Measurements. - VNMAR = 13, ///< Asynchronous output type is Magnetic, Acceleration, and Angular Rate Measurements. - VNYMR = 14, ///< Asynchronous output type is Yaw, Pitch, Roll, Magnetic, Acceleration, and Angular Rate Measurements. - #ifdef INTERNAL - VNYCM = 15, ///< Asynchronous output type is Yaw, Pitch, Roll, and Calibrated Measurements. - #endif - VNYBA = 16, ///< Asynchronous output type is Yaw, Pitch, Roll, Body True Acceleration. - VNYIA = 17, ///< Asynchronous output type is Yaw, Pitch, Roll, Inertial True Acceleration. - #ifdef INTERNAL - VNICM = 18, ///< Asynchronous output type is Yaw, Pitch, Roll, Inertial Magnetic/Acceleration, and Angular Rate Measurements. - #endif - VNIMU = 19, ///< Asynchronous output type is Calibrated Inertial Measurements. - VNGPS = 20, ///< Asynchronous output type is GPS LLA. - VNGPE = 21, ///< Asynchronous output type is GPS ECEF. - VNINS = 22, ///< Asynchronous output type is INS LLA solution. - VNINE = 23, ///< Asynchronous output type is INS ECEF solution. - VNISL = 28, ///< Asynchronous output type is INS LLA 2 solution. - VNISE = 29, ///< Asynchronous output type is INS ECEF 2 solution. - VNDTV = 30, ///< Asynchronous output type is Delta Theta and Delta Velocity. - VNG2S = 32, ///< Asynchronous output type is GPS LLA. - VNG2E = 33, ///< Asynchronous output type is GPS ECEF. - - #ifdef INTERNAL - VNRAW = 252, ///< Asynchronous output type is Raw Voltage Measurements. - VNCMV = 253, ///< Asynchronous output type is Calibrated Measurements. - VNSTV = 254, ///< Asynchronous output type is Kalman Filter State Vector. - VNCOV = 255, ///< Asynchronous output type is Kalman Filter Covariance Matrix Diagonal. - #endif +enum AsciiAsync { + VNOFF = 0, ///< Asynchronous output is turned off. + VNYPR = 1, ///< Asynchronous output type is Yaw, Pitch, Roll. + VNQTN = 2, ///< Asynchronous output type is Quaternion. +#ifdef INTERNAL + VNQTM = 3, ///< Asynchronous output type is Quaternion and Magnetic. + VNQTA = 4, ///< Asynchronous output type is Quaternion and Acceleration. + VNQTR = 5, ///< Asynchronous output type is Quaternion and Angular Rates. + VNQMA = 6, ///< Asynchronous output type is Quaternion, Magnetic and Acceleration. + VNQAR = 7, ///< Asynchronous output type is Quaternion, Acceleration and Angular Rates. +#endif + VNQMR = 8, ///< Asynchronous output type is Quaternion, Magnetic, Acceleration and Angular Rates. +#ifdef INTERNAL + VNDCM = 9, ///< Asynchronous output type is Directional Cosine Orientation Matrix. +#endif + VNMAG = 10, ///< Asynchronous output type is Magnetic Measurements. + VNACC = 11, ///< Asynchronous output type is Acceleration Measurements. + VNGYR = 12, ///< Asynchronous output type is Angular Rate Measurements. + VNMAR = + 13, ///< Asynchronous output type is Magnetic, Acceleration, and Angular Rate Measurements. + VNYMR = + 14, ///< Asynchronous output type is Yaw, Pitch, Roll, Magnetic, Acceleration, and Angular Rate Measurements. +#ifdef INTERNAL + VNYCM = 15, ///< Asynchronous output type is Yaw, Pitch, Roll, and Calibrated Measurements. +#endif + VNYBA = 16, ///< Asynchronous output type is Yaw, Pitch, Roll, Body True Acceleration. + VNYIA = 17, ///< Asynchronous output type is Yaw, Pitch, Roll, Inertial True Acceleration. +#ifdef INTERNAL + VNICM = + 18, ///< Asynchronous output type is Yaw, Pitch, Roll, Inertial Magnetic/Acceleration, and Angular Rate Measurements. +#endif + VNIMU = 19, ///< Asynchronous output type is Calibrated Inertial Measurements. + VNGPS = 20, ///< Asynchronous output type is GPS LLA. + VNGPE = 21, ///< Asynchronous output type is GPS ECEF. + VNINS = 22, ///< Asynchronous output type is INS LLA solution. + VNINE = 23, ///< Asynchronous output type is INS ECEF solution. + VNISL = 28, ///< Asynchronous output type is INS LLA 2 solution. + VNISE = 29, ///< Asynchronous output type is INS ECEF 2 solution. + VNDTV = 30, ///< Asynchronous output type is Delta Theta and Delta Velocity. + VNG2S = 32, ///< Asynchronous output type is GPS LLA. + VNG2E = 33, ///< Asynchronous output type is GPS ECEF. + +#ifdef INTERNAL + VNRAW = 252, ///< Asynchronous output type is Raw Voltage Measurements. + VNCMV = 253, ///< Asynchronous output type is Calibrated Measurements. + VNSTV = 254, ///< Asynchronous output type is Kalman Filter State Vector. + VNCOV = 255, ///< Asynchronous output type is Kalman Filter Covariance Matrix Diagonal. +#endif }; /// \brief Async modes for the Binary Output registers. -enum AsyncMode -{ - ASYNCMODE_NONE = 0, ///< None. - ASYNCMODE_PORT1 = 1, ///< Serial port 1. - ASYNCMODE_PORT2 = 2, ///< Serial port 2. - ASYNCMODE_BOTH = 3 ///< Both serial ports. +enum AsyncMode { + ASYNCMODE_NONE = 0, ///< None. + ASYNCMODE_PORT1 = 1, ///< Serial port 1. + ASYNCMODE_PORT2 = 2, ///< Serial port 2. + ASYNCMODE_BOTH = 3 ///< Both serial ports. }; /// \brief The available binary output groups. -enum BinaryGroup -{ - BINARYGROUP_COMMON = 0x01, ///< Common group. - BINARYGROUP_TIME = 0x02, ///< Time group. - BINARYGROUP_IMU = 0x04, ///< IMU group. - BINARYGROUP_GPS = 0x08, ///< GPS group. - BINARYGROUP_ATTITUDE = 0x10, ///< Attitude group. - BINARYGROUP_INS = 0x20, ///< INS group. - BINARYGROUP_GPS2 = 0x40 ///< GPS2 group. +enum BinaryGroup { + BINARYGROUP_COMMON = 0x01, ///< Common group. + BINARYGROUP_TIME = 0x02, ///< Time group. + BINARYGROUP_IMU = 0x04, ///< IMU group. + BINARYGROUP_GPS = 0x08, ///< GPS group. + BINARYGROUP_ATTITUDE = 0x10, ///< Attitude group. + BINARYGROUP_INS = 0x20, ///< INS group. + BINARYGROUP_GPS2 = 0x40 ///< GPS2 group. }; /// \brief Flags for the binary group 1 'Common' in the binary output registers. -enum CommonGroup -{ - COMMONGROUP_NONE = 0x0000, ///< None. - COMMONGROUP_TIMESTARTUP = 0x0001, ///< TimeStartup. - COMMONGROUP_TIMEGPS = 0x0002, ///< TimeGps. - COMMONGROUP_TIMESYNCIN = 0x0004, ///< TimeSyncIn. - COMMONGROUP_YAWPITCHROLL = 0x0008, ///< YawPitchRoll. - COMMONGROUP_QUATERNION = 0x0010, ///< Quaternion. - COMMONGROUP_ANGULARRATE = 0x0020, ///< AngularRate. - COMMONGROUP_POSITION = 0x0040, ///< Position. - COMMONGROUP_VELOCITY = 0x0080, ///< Velocity. - COMMONGROUP_ACCEL = 0x0100, ///< Accel. - COMMONGROUP_IMU = 0x0200, ///< Imu. - COMMONGROUP_MAGPRES = 0x0400, ///< MagPres. - COMMONGROUP_DELTATHETA = 0x0800, ///< DeltaTheta. - COMMONGROUP_INSSTATUS = 0x1000, ///< InsStatus. - COMMONGROUP_SYNCINCNT = 0x2000, ///< SyncInCnt. - COMMONGROUP_TIMEGPSPPS = 0x4000 ///< TimeGpsPps. +enum CommonGroup { + COMMONGROUP_NONE = 0x0000, ///< None. + COMMONGROUP_TIMESTARTUP = 0x0001, ///< TimeStartup. + COMMONGROUP_TIMEGPS = 0x0002, ///< TimeGps. + COMMONGROUP_TIMESYNCIN = 0x0004, ///< TimeSyncIn. + COMMONGROUP_YAWPITCHROLL = 0x0008, ///< YawPitchRoll. + COMMONGROUP_QUATERNION = 0x0010, ///< Quaternion. + COMMONGROUP_ANGULARRATE = 0x0020, ///< AngularRate. + COMMONGROUP_POSITION = 0x0040, ///< Position. + COMMONGROUP_VELOCITY = 0x0080, ///< Velocity. + COMMONGROUP_ACCEL = 0x0100, ///< Accel. + COMMONGROUP_IMU = 0x0200, ///< Imu. + COMMONGROUP_MAGPRES = 0x0400, ///< MagPres. + COMMONGROUP_DELTATHETA = 0x0800, ///< DeltaTheta. + COMMONGROUP_INSSTATUS = 0x1000, ///< InsStatus. + COMMONGROUP_SYNCINCNT = 0x2000, ///< SyncInCnt. + COMMONGROUP_TIMEGPSPPS = 0x4000 ///< TimeGpsPps. }; /// \brief Flags for the binary group 2 'Time' in the binary output registers. -enum TimeGroup -{ - TIMEGROUP_NONE = 0x0000, ///< None. - TIMEGROUP_TIMESTARTUP = 0x0001, ///< TimeStartup. - TIMEGROUP_TIMEGPS = 0x0002, ///< TimeGps. - TIMEGROUP_GPSTOW = 0x0004, ///< GpsTow. - TIMEGROUP_GPSWEEK = 0x0008, ///< GpsWeek. - TIMEGROUP_TIMESYNCIN = 0x0010, ///< TimeSyncIn. - TIMEGROUP_TIMEGPSPPS = 0x0020, ///< TimeGpsPps. - TIMEGROUP_TIMEUTC = 0x0040, ///< TimeUTC. - TIMEGROUP_SYNCINCNT = 0x0080, ///< SyncInCnt. - TIMEGROUP_SYNCOUTCNT = 0x0100, ///< SyncOutCnt. - TIMEGROUP_TIMESTATUS = 0x0200 ///< TimeStatus. +enum TimeGroup { + TIMEGROUP_NONE = 0x0000, ///< None. + TIMEGROUP_TIMESTARTUP = 0x0001, ///< TimeStartup. + TIMEGROUP_TIMEGPS = 0x0002, ///< TimeGps. + TIMEGROUP_GPSTOW = 0x0004, ///< GpsTow. + TIMEGROUP_GPSWEEK = 0x0008, ///< GpsWeek. + TIMEGROUP_TIMESYNCIN = 0x0010, ///< TimeSyncIn. + TIMEGROUP_TIMEGPSPPS = 0x0020, ///< TimeGpsPps. + TIMEGROUP_TIMEUTC = 0x0040, ///< TimeUTC. + TIMEGROUP_SYNCINCNT = 0x0080, ///< SyncInCnt. + TIMEGROUP_SYNCOUTCNT = 0x0100, ///< SyncOutCnt. + TIMEGROUP_TIMESTATUS = 0x0200 ///< TimeStatus. }; /// \brief Flags for the binary group 3 'IMU' in the binary output registers. -enum ImuGroup -{ - IMUGROUP_NONE = 0x0000, ///< None. - IMUGROUP_IMUSTATUS = 0x0001, ///< ImuStatus. - IMUGROUP_UNCOMPMAG = 0x0002, ///< UncompMag. - IMUGROUP_UNCOMPACCEL = 0x0004, ///< UncompAccel. - IMUGROUP_UNCOMPGYRO = 0x0008, ///< UncompGyro. - IMUGROUP_TEMP = 0x0010, ///< Temp. - IMUGROUP_PRES = 0x0020, ///< Pres. - IMUGROUP_DELTATHETA = 0x0040, ///< DeltaTheta. - IMUGROUP_DELTAVEL = 0x0080, ///< DeltaVel. - IMUGROUP_MAG = 0x0100, ///< Mag. - IMUGROUP_ACCEL = 0x0200, ///< Accel. - IMUGROUP_ANGULARRATE = 0x0400, ///< AngularRate. - IMUGROUP_SENSSAT = 0x0800, ///< SensSat. +enum ImuGroup { + IMUGROUP_NONE = 0x0000, ///< None. + IMUGROUP_IMUSTATUS = 0x0001, ///< ImuStatus. + IMUGROUP_UNCOMPMAG = 0x0002, ///< UncompMag. + IMUGROUP_UNCOMPACCEL = 0x0004, ///< UncompAccel. + IMUGROUP_UNCOMPGYRO = 0x0008, ///< UncompGyro. + IMUGROUP_TEMP = 0x0010, ///< Temp. + IMUGROUP_PRES = 0x0020, ///< Pres. + IMUGROUP_DELTATHETA = 0x0040, ///< DeltaTheta. + IMUGROUP_DELTAVEL = 0x0080, ///< DeltaVel. + IMUGROUP_MAG = 0x0100, ///< Mag. + IMUGROUP_ACCEL = 0x0200, ///< Accel. + IMUGROUP_ANGULARRATE = 0x0400, ///< AngularRate. + IMUGROUP_SENSSAT = 0x0800, ///< SensSat. }; /// \brief Flags for the binary group 4 'GPS' and group 7 'GPS2' in the binary output registers. -enum GpsGroup -{ - GPSGROUP_NONE = 0x0000, ///< None. - GPSGROUP_UTC = 0x0001, ///< UTC. - GPSGROUP_TOW = 0x0002, ///< Tow. - GPSGROUP_WEEK = 0x0004, ///< Week. - GPSGROUP_NUMSATS = 0x0008, ///< NumSats. - GPSGROUP_FIX = 0x0010, ///< Fix. - GPSGROUP_POSLLA = 0x0020, ///< PosLla. - GPSGROUP_POSECEF = 0x0040, ///< PosEcef. - GPSGROUP_VELNED = 0x0080, ///< VelNed. - GPSGROUP_VELECEF = 0x0100, ///< VelEcef. - GPSGROUP_POSU = 0x0200, ///< PosU. - GPSGROUP_VELU = 0x0400, ///< VelU. - GPSGROUP_TIMEU = 0x0800, ///< TimeU. - GPSGROUP_TIMEINFO = 0x1000, ///< TimeInfo. - GPSGROUP_DOP = 0x2000, ///< Dop. +enum GpsGroup { + GPSGROUP_NONE = 0x0000, ///< None. + GPSGROUP_UTC = 0x0001, ///< UTC. + GPSGROUP_TOW = 0x0002, ///< Tow. + GPSGROUP_WEEK = 0x0004, ///< Week. + GPSGROUP_NUMSATS = 0x0008, ///< NumSats. + GPSGROUP_FIX = 0x0010, ///< Fix. + GPSGROUP_POSLLA = 0x0020, ///< PosLla. + GPSGROUP_POSECEF = 0x0040, ///< PosEcef. + GPSGROUP_VELNED = 0x0080, ///< VelNed. + GPSGROUP_VELECEF = 0x0100, ///< VelEcef. + GPSGROUP_POSU = 0x0200, ///< PosU. + GPSGROUP_VELU = 0x0400, ///< VelU. + GPSGROUP_TIMEU = 0x0800, ///< TimeU. + GPSGROUP_TIMEINFO = 0x1000, ///< TimeInfo. + GPSGROUP_DOP = 0x2000, ///< Dop. }; /// \brief Flags for the binary group 5 'Attitude' in the binary output registers. -enum AttitudeGroup -{ - ATTITUDEGROUP_NONE = 0x0000, ///< None. - ATTITUDEGROUP_VPESTATUS = 0x0001, ///< VpeStatus. - ATTITUDEGROUP_YAWPITCHROLL = 0x0002, ///< YawPitchRoll. - ATTITUDEGROUP_QUATERNION = 0x0004, ///< Quaternion. - ATTITUDEGROUP_DCM = 0x0008, ///< DCM. - ATTITUDEGROUP_MAGNED = 0x0010, ///< MagNed. - ATTITUDEGROUP_ACCELNED = 0x0020, ///< AccelNed. - ATTITUDEGROUP_LINEARACCELBODY = 0x0040, ///< LinearAccelBody. - ATTITUDEGROUP_LINEARACCELNED = 0x0080, ///< LinearAccelNed. - ATTITUDEGROUP_YPRU = 0x0100, ///< YprU. - ATTITUDEGROUP_HEAVE = 0x1000, ///< Heave. +enum AttitudeGroup { + ATTITUDEGROUP_NONE = 0x0000, ///< None. + ATTITUDEGROUP_VPESTATUS = 0x0001, ///< VpeStatus. + ATTITUDEGROUP_YAWPITCHROLL = 0x0002, ///< YawPitchRoll. + ATTITUDEGROUP_QUATERNION = 0x0004, ///< Quaternion. + ATTITUDEGROUP_DCM = 0x0008, ///< DCM. + ATTITUDEGROUP_MAGNED = 0x0010, ///< MagNed. + ATTITUDEGROUP_ACCELNED = 0x0020, ///< AccelNed. + ATTITUDEGROUP_LINEARACCELBODY = 0x0040, ///< LinearAccelBody. + ATTITUDEGROUP_LINEARACCELNED = 0x0080, ///< LinearAccelNed. + ATTITUDEGROUP_YPRU = 0x0100, ///< YprU. + ATTITUDEGROUP_HEAVE = 0x1000, ///< Heave. }; /// \brief Flags for the binary group 6 'INS' in the binary output registers. -enum InsGroup -{ - INSGROUP_NONE = 0x0000, ///< None. - INSGROUP_INSSTATUS = 0x0001, ///< InsStatus. - INSGROUP_POSLLA = 0x0002, ///< PosLla. - INSGROUP_POSECEF = 0x0004, ///< PosEcef. - INSGROUP_VELBODY = 0x0008, ///< VelBody. - INSGROUP_VELNED = 0x0010, ///< VelNed. - INSGROUP_VELECEF = 0x0020, ///< VelEcef. - INSGROUP_MAGECEF = 0x0040, ///< MagEcef. - INSGROUP_ACCELECEF = 0x0080, ///< AccelEcef. - INSGROUP_LINEARACCELECEF = 0x0100, ///< LinearAccelEcef. - INSGROUP_POSU = 0x0200, ///< PosU. - INSGROUP_VELU = 0x0400, ///< VelU. +enum InsGroup { + INSGROUP_NONE = 0x0000, ///< None. + INSGROUP_INSSTATUS = 0x0001, ///< InsStatus. + INSGROUP_POSLLA = 0x0002, ///< PosLla. + INSGROUP_POSECEF = 0x0004, ///< PosEcef. + INSGROUP_VELBODY = 0x0008, ///< VelBody. + INSGROUP_VELNED = 0x0010, ///< VelNed. + INSGROUP_VELECEF = 0x0020, ///< VelEcef. + INSGROUP_MAGECEF = 0x0040, ///< MagEcef. + INSGROUP_ACCELECEF = 0x0080, ///< AccelEcef. + INSGROUP_LINEARACCELECEF = 0x0100, ///< LinearAccelEcef. + INSGROUP_POSU = 0x0200, ///< PosU. + INSGROUP_VELU = 0x0400, ///< VelU. }; /// \brief Errors that the VectorNav sensor can report. -enum SensorError -{ - ERR_HARD_FAULT = 1, ///< Hard fault. - ERR_SERIAL_BUFFER_OVERFLOW = 2, ///< Serial buffer overflow. - ERR_INVALID_CHECKSUM = 3, ///< Invalid checksum. - ERR_INVALID_COMMAND = 4, ///< Invalid command. - ERR_NOT_ENOUGH_PARAMETERS = 5, ///< Not enough parameters. - ERR_TOO_MANY_PARAMETERS = 6, ///< Too many parameters. - ERR_INVALID_PARAMETER = 7, ///< Invalid parameter. - ERR_INVALID_REGISTER = 8, ///< Invalid register. - ERR_UNAUTHORIZED_ACCESS = 9, ///< Unauthorized access. - ERR_WATCHDOG_RESET = 10, ///< Watchdog reset. - ERR_OUTPUT_BUFFER_OVERFLOW = 11, ///< Output buffer overflow. - ERR_INSUFFICIENT_BAUD_RATE = 12, ///< Insufficient baud rate. - ERR_ERROR_BUFFER_OVERFLOW = 255 ///< Error buffer overflow. -}; - -enum BootloaderError -{ - BLE_NONE = 0, ///< No Error - BLE_INVALID_COMMAND = 1, ///< Problem with VNX record, abort - BLE_INVALID_RECORD_TYPE = 2, ///< Problem with VNX record, abort - BLE_INVALID_BYTE_COUNT = 3, ///< Problem with VNX record, abort - BLE_INVALID_MEMORY_ADDRESS = 4, ///< Problem with VNX record, abort - BLE_COMM_ERROR = 5, ///< Checksum error, resend record - BLE_INVALID_HEX_FILE = 6, ///< Problem with VNX record, abort - BLE_DECRYPTION_ERROR = 7, ///< Invalid VNX file or record sent out of order, abort - BLE_INVALID_BLOCK_CRC = 8, ///< Data verification failed, abort - BLE_INVALID_PROGRAM_CRC = 9, ///< Problemw ith firmware on device - BLE_INVALID_PROGRAM_SIZE = 10, ///< Problemw ith firmware on device - BLE_MAX_RETRY_COUNT = 11, ///< Too many errors, abort - BLE_TIMEOUT = 12, ///< Timeout expired, reset - BLE_RESERVED = 13 ///< Contact VectorNav, abort +enum SensorError { + ERR_HARD_FAULT = 1, ///< Hard fault. + ERR_SERIAL_BUFFER_OVERFLOW = 2, ///< Serial buffer overflow. + ERR_INVALID_CHECKSUM = 3, ///< Invalid checksum. + ERR_INVALID_COMMAND = 4, ///< Invalid command. + ERR_NOT_ENOUGH_PARAMETERS = 5, ///< Not enough parameters. + ERR_TOO_MANY_PARAMETERS = 6, ///< Too many parameters. + ERR_INVALID_PARAMETER = 7, ///< Invalid parameter. + ERR_INVALID_REGISTER = 8, ///< Invalid register. + ERR_UNAUTHORIZED_ACCESS = 9, ///< Unauthorized access. + ERR_WATCHDOG_RESET = 10, ///< Watchdog reset. + ERR_OUTPUT_BUFFER_OVERFLOW = 11, ///< Output buffer overflow. + ERR_INSUFFICIENT_BAUD_RATE = 12, ///< Insufficient baud rate. + ERR_ERROR_BUFFER_OVERFLOW = 255 ///< Error buffer overflow. +}; + +enum BootloaderError { + BLE_NONE = 0, ///< No Error + BLE_INVALID_COMMAND = 1, ///< Problem with VNX record, abort + BLE_INVALID_RECORD_TYPE = 2, ///< Problem with VNX record, abort + BLE_INVALID_BYTE_COUNT = 3, ///< Problem with VNX record, abort + BLE_INVALID_MEMORY_ADDRESS = 4, ///< Problem with VNX record, abort + BLE_COMM_ERROR = 5, ///< Checksum error, resend record + BLE_INVALID_HEX_FILE = 6, ///< Problem with VNX record, abort + BLE_DECRYPTION_ERROR = 7, ///< Invalid VNX file or record sent out of order, abort + BLE_INVALID_BLOCK_CRC = 8, ///< Data verification failed, abort + BLE_INVALID_PROGRAM_CRC = 9, ///< Problemw ith firmware on device + BLE_INVALID_PROGRAM_SIZE = 10, ///< Problemw ith firmware on device + BLE_MAX_RETRY_COUNT = 11, ///< Too many errors, abort + BLE_TIMEOUT = 12, ///< Timeout expired, reset + BLE_RESERVED = 13 ///< Contact VectorNav, abort }; /// \brief Different modes for the SyncInMode field of the Synchronization Control register. -enum SyncInMode -{ - #ifdef INTERNAL - /// \brief Count the number of trigger events on SYNC_IN_2 pin. - /// \deprecated This option is obsolete for VN-100 firmware version 2.0 and greater and VN-200 firmware version 1.0 and greater. - SYNCINMODE_COUNT2 = 0, - /// \brief Start ADC sampling on trigger of SYNC_IN_2 pin. - /// \deprecated This option is obsolete for VN-100 firmware version 2.0 and greater and VN-200 firmware version 1.0 and greater. - SYNCINMODE_ADC2 = 1, - /// \brief Output asynchronous message on trigger of SYNC_IN_2 pin. - /// \deprecated This option is obsolete for VN-100 firmware version 2.0 and greater and VN-200 firmware version 1.0 and greater. - SYNCINMODE_ASYNC2 = 2, - #endif - /// \brief Count number of trigger events on SYNC_IN pin. - SYNCINMODE_COUNT = 3, - /// \brief Start IMU sampling on trigger of SYNC_IN pin. - SYNCINMODE_IMU = 4, - /// \brief Output asynchronous message on trigger of SYNC_IN pin. - SYNCINMODE_ASYNC = 5, - /// \brief Output asynchronous 0Hz message on trigger of SYNC_IN pin. - SYNCINMODE_ASYNC3 = 6 +enum SyncInMode { +#ifdef INTERNAL + /// \brief Count the number of trigger events on SYNC_IN_2 pin. + /// \deprecated This option is obsolete for VN-100 firmware version 2.0 and greater and VN-200 firmware version 1.0 and greater. + SYNCINMODE_COUNT2 = 0, + /// \brief Start ADC sampling on trigger of SYNC_IN_2 pin. + /// \deprecated This option is obsolete for VN-100 firmware version 2.0 and greater and VN-200 firmware version 1.0 and greater. + SYNCINMODE_ADC2 = 1, + /// \brief Output asynchronous message on trigger of SYNC_IN_2 pin. + /// \deprecated This option is obsolete for VN-100 firmware version 2.0 and greater and VN-200 firmware version 1.0 and greater. + SYNCINMODE_ASYNC2 = 2, +#endif + /// \brief Count number of trigger events on SYNC_IN pin. + SYNCINMODE_COUNT = 3, + /// \brief Start IMU sampling on trigger of SYNC_IN pin. + SYNCINMODE_IMU = 4, + /// \brief Output asynchronous message on trigger of SYNC_IN pin. + SYNCINMODE_ASYNC = 5, + /// \brief Output asynchronous 0Hz message on trigger of SYNC_IN pin. + SYNCINMODE_ASYNC3 = 6 }; /// \brief Different modes for the SyncInEdge field of the Synchronization Control register. -enum SyncInEdge -{ - /// \brief Trigger on the rising edge on the SYNC_IN pin. - SYNCINEDGE_RISING = 0, - /// \brief Trigger on the falling edge on the SYNC_IN pin. - SYNCINEDGE_FALLING = 1 +enum SyncInEdge { + /// \brief Trigger on the rising edge on the SYNC_IN pin. + SYNCINEDGE_RISING = 0, + /// \brief Trigger on the falling edge on the SYNC_IN pin. + SYNCINEDGE_FALLING = 1 }; /// \brief Different modes for the SyncOutMode field of the Synchronization Control register. -enum SyncOutMode -{ - /// \brief None. - SYNCOUTMODE_NONE = 0, - /// \brief Trigger at start of IMU sampling. - SYNCOUTMODE_ITEMSTART = 1, - /// \brief Trigger when IMU measurements are available. - SYNCOUTMODE_IMUREADY = 2, - /// \brief Trigger when attitude measurements are available. - SYNCOUTMODE_INS = 3, - /// \brief Trigger on GPS PPS event when a 3D fix is valid. - SYNCOUTMODE_GPSPPS = 6 +enum SyncOutMode { + /// \brief None. + SYNCOUTMODE_NONE = 0, + /// \brief Trigger at start of IMU sampling. + SYNCOUTMODE_ITEMSTART = 1, + /// \brief Trigger when IMU measurements are available. + SYNCOUTMODE_IMUREADY = 2, + /// \brief Trigger when attitude measurements are available. + SYNCOUTMODE_INS = 3, + /// \brief Trigger on GPS PPS event when a 3D fix is valid. + SYNCOUTMODE_GPSPPS = 6 }; /// \brief Different modes for the SyncOutPolarity field of the Synchronization Control register. -enum SyncOutPolarity -{ - /// \brief Negative pulse. - SYNCOUTPOLARITY_NEGATIVE = 0, - /// \brief Positive pulse. - SYNCOUTPOLARITY_POSITIVE = 1 +enum SyncOutPolarity { + /// \brief Negative pulse. + SYNCOUTPOLARITY_NEGATIVE = 0, + /// \brief Positive pulse. + SYNCOUTPOLARITY_POSITIVE = 1 }; /// \brief Counting modes for the Communication Protocol Control register. -enum CountMode -{ - /// \brief Off. - COUNTMODE_NONE = 0, - /// \brief SyncIn counter. - COUNTMODE_SYNCINCOUNT = 1, - /// \brief SyncIn time. - COUNTMODE_SYNCINTIME = 2, - /// \brief SyncOut counter. - COUNTMODE_SYNCOUTCOUNTER = 3, - /// \brief GPS PPS time. - COUNTMODE_GPSPPS = 4 +enum CountMode { + /// \brief Off. + COUNTMODE_NONE = 0, + /// \brief SyncIn counter. + COUNTMODE_SYNCINCOUNT = 1, + /// \brief SyncIn time. + COUNTMODE_SYNCINTIME = 2, + /// \brief SyncOut counter. + COUNTMODE_SYNCOUTCOUNTER = 3, + /// \brief GPS PPS time. + COUNTMODE_GPSPPS = 4 }; /// \brief Status modes for the Communication Protocol Control register. -enum StatusMode -{ - /// \brief Off. - STATUSMODE_OFF = 0, - /// \brief VPE status. - STATUSMODE_VPESTATUS = 1, - /// \brief INS status. - STATUSMODE_INSSTATUS = 2 +enum StatusMode { + /// \brief Off. + STATUSMODE_OFF = 0, + /// \brief VPE status. + STATUSMODE_VPESTATUS = 1, + /// \brief INS status. + STATUSMODE_INSSTATUS = 2 }; /// \brief Checksum modes for the Communication Protocol Control register. -enum ChecksumMode -{ - /// \brief Off. - CHECKSUMMODE_OFF = 0, - /// \brief 8-bit checksum. - CHECKSUMMODE_CHECKSUM = 1, - /// \brief 16-bit CRC. - CHECKSUMMODE_CRC = 2 +enum ChecksumMode { + /// \brief Off. + CHECKSUMMODE_OFF = 0, + /// \brief 8-bit checksum. + CHECKSUMMODE_CHECKSUM = 1, + /// \brief 16-bit CRC. + CHECKSUMMODE_CRC = 2 }; /// \brief Error modes for the Communication Protocol Control register. -enum ErrorMode -{ - /// \brief Ignore error. - ERRORMODE_IGNORE = 0, - /// \brief Send error. - ERRORMODE_SEND = 1, - /// \brief Send error and set ADOR register to off. - ERRORMODE_SENDANDOFF = 2 +enum ErrorMode { + /// \brief Ignore error. + ERRORMODE_IGNORE = 0, + /// \brief Send error. + ERRORMODE_SEND = 1, + /// \brief Send error and set ADOR register to off. + ERRORMODE_SENDANDOFF = 2 }; /// \brief Filter modes for the IMU Filtering Configuration register. -enum FilterMode -{ - /// \brief No filtering. - FILTERMODE_NOFILTERING = 0, - /// \brief Filtering performed only on raw uncompensated IMU measurements. - FILTERMODE_ONLYRAW = 1, - /// \brief Filtering performed only on compensated IMU measurements. - FILTERMODE_ONLYCOMPENSATED = 2, - /// \brief Filtering performed on both uncompensated and compensated IMU measurements. - FILTERMODE_BOTH = 3 +enum FilterMode { + /// \brief No filtering. + FILTERMODE_NOFILTERING = 0, + /// \brief Filtering performed only on raw uncompensated IMU measurements. + FILTERMODE_ONLYRAW = 1, + /// \brief Filtering performed only on compensated IMU measurements. + FILTERMODE_ONLYCOMPENSATED = 2, + /// \brief Filtering performed on both uncompensated and compensated IMU measurements. + FILTERMODE_BOTH = 3 }; /// \brief Integration frames for the Delta Theta and Delta Velocity Configuration register. -enum IntegrationFrame -{ - /// \brief Body frame. - INTEGRATIONFRAME_BODY = 0, - /// \brief NED frame. - INTEGRATIONFRAME_NED = 1 +enum IntegrationFrame { + /// \brief Body frame. + INTEGRATIONFRAME_BODY = 0, + /// \brief NED frame. + INTEGRATIONFRAME_NED = 1 }; /// \brief Gyro compensation modes for the Delta Theta and Delta Velocity configuration register. -enum CompensationMode -{ - /// \brief None. - COMPENSATIONMODE_NONE = 0, - /// \brief Bias. - COMPENSATIONMODE_BIAS = 1 +enum CompensationMode { + /// \brief None. + COMPENSATIONMODE_NONE = 0, + /// \brief Bias. + COMPENSATIONMODE_BIAS = 1 }; /// \brief Accelerometer compensation modes for the Delta Theta and Delta Velocity configuration register. -enum AccCompensationMode -{ - /// \brief None. - ACCCOMPENSATIONMODE_NONE = 0, - /// \brief Gravity. - ACCCOMPENSATIONMODE_GRAV = 1, - /// \brief Bias. - ACCCOMPENSATIONMODE_BIAS = 2, - /// \brief Bias + gravity. - ACCCOMPENSATIONMODE_BIASGRAV = 3 +enum AccCompensationMode { + /// \brief None. + ACCCOMPENSATIONMODE_NONE = 0, + /// \brief Gravity. + ACCCOMPENSATIONMODE_GRAV = 1, + /// \brief Bias. + ACCCOMPENSATIONMODE_BIAS = 2, + /// \brief Bias + gravity. + ACCCOMPENSATIONMODE_BIASGRAV = 3 }; - /// \brief Earth's angular rate correction for the Delta Theta and Delta Velocity configuration register. -enum EarthRateCorrection -{ - /// \brief None. - EARTHRATECORR_NONE = 0, - /// \brief Angular rate. - EARTHRATECORR_RATE = 1, - /// \brief Coriolis acceleration. - EARTHRATECORR_CORIOLIS = 2, - /// \brief Angular rate + coriolis acceleration. - EARTHRATECORR_RATECOR = 3 +enum EarthRateCorrection { + /// \brief None. + EARTHRATECORR_NONE = 0, + /// \brief Angular rate. + EARTHRATECORR_RATE = 1, + /// \brief Coriolis acceleration. + EARTHRATECORR_CORIOLIS = 2, + /// \brief Angular rate + coriolis acceleration. + EARTHRATECORR_RATECOR = 3 }; /// \brief GPS fix modes for the GPS Solution - LLA register. -enum GpsFix -{ - /// \brief No fix. - GPSFIX_NOFIX = 0, - /// \brief Time only. - GPSFIX_TIMEONLY = 1, - /// \brief 2D. - GPSFIX_2D = 2, - /// \brief 3D. - GPSFIX_3D = 3 +enum GpsFix { + /// \brief No fix. + GPSFIX_NOFIX = 0, + /// \brief Time only. + GPSFIX_TIMEONLY = 1, + /// \brief 2D. + GPSFIX_2D = 2, + /// \brief 3D. + GPSFIX_3D = 3 }; /// \brief GPS modes for the GPS Configuration register. -enum GpsMode -{ - /// \brief Use onboard GPS. - GPSMODE_ONBOARDGPS = 0, - /// \brief Use external GPS. - GPSMODE_EXTERNALGPS = 1, - /// \brief Use external VN-200 as GPS. - GPSMODE_EXTERNALVN200GPS = 2 +enum GpsMode { + /// \brief Use onboard GPS. + GPSMODE_ONBOARDGPS = 0, + /// \brief Use external GPS. + GPSMODE_EXTERNALGPS = 1, + /// \brief Use external VN-200 as GPS. + GPSMODE_EXTERNALVN200GPS = 2 }; /// \brief GPS PPS mode for the GPS Configuration register. -enum PpsSource -{ - /// \brief GPS PPS signal on GPS_PPS pin and triggered on rising edge. - PPSSOURCE_GPSPPSRISING = 0, - /// \brief GPS PPS signal on GPS_PPS pin and triggered on falling edge. - PPSSOURCE_GPSPPSFALLING = 1, - /// \brief GPS PPS signal on SyncIn pin and triggered on rising edge. - PPSSOURCE_SYNCINRISING = 2, - /// \brief GPS PPS signal on SyncIn pin and triggered on falling edge. - PPSSOURCE_SYNCINFALLING = 3 +enum PpsSource { + /// \brief GPS PPS signal on GPS_PPS pin and triggered on rising edge. + PPSSOURCE_GPSPPSRISING = 0, + /// \brief GPS PPS signal on GPS_PPS pin and triggered on falling edge. + PPSSOURCE_GPSPPSFALLING = 1, + /// \brief GPS PPS signal on SyncIn pin and triggered on rising edge. + PPSSOURCE_SYNCINRISING = 2, + /// \brief GPS PPS signal on SyncIn pin and triggered on falling edge. + PPSSOURCE_SYNCINFALLING = 3 }; /// \brief GPS Rate for the GPS Configuration register. -enum GpsRate -{ - /// \brief GPS update rate : 1 Hz - GPSRATE_1HZ = 1, - /// \brief GPS update rate : 5 Hz - GPSRATE_5HZ = 5 +enum GpsRate { + /// \brief GPS update rate : 1 Hz + GPSRATE_1HZ = 1, + /// \brief GPS update rate : 5 Hz + GPSRATE_5HZ = 5 }; /// \brief GPS Antenna Power mode for the GPS Configuration register. -enum AntPower -{ - /// \brief GPS antenna power source: off/resv (reserved as 0 on some Industrial Series firmwares) - ANTPOWER_OFFRESV = 0, - /// \brief GPS antenna,power source: internal - ANTPOWER_INTERNAL = 1, - /// \brief GPS antenna power source: external - ANTPOWER_EXTERNAL = 2 +enum AntPower { + /// \brief GPS antenna power source: off/resv (reserved as 0 on some Industrial Series firmwares) + ANTPOWER_OFFRESV = 0, + /// \brief GPS antenna,power source: internal + ANTPOWER_INTERNAL = 1, + /// \brief GPS antenna power source: external + ANTPOWER_EXTERNAL = 2 }; /// \brief VPE Enable mode for the VPE Basic Control register. -enum VpeEnable -{ - /// \brief Disable - VPEENABLE_DISABLE = 0, - /// \brief Enable - VPEENABLE_ENABLE = 1 +enum VpeEnable { + /// \brief Disable + VPEENABLE_DISABLE = 0, + /// \brief Enable + VPEENABLE_ENABLE = 1 }; /// \brief VPE Heading modes used by the VPE Basic Control register. -enum HeadingMode -{ - /// \brief Absolute heading. - HEADINGMODE_ABSOLUTE = 0, - /// \brief Relative heading. - HEADINGMODE_RELATIVE = 1, - /// \brief Indoor heading. - HEADINGMODE_INDOOR = 2 +enum HeadingMode { + /// \brief Absolute heading. + HEADINGMODE_ABSOLUTE = 0, + /// \brief Relative heading. + HEADINGMODE_RELATIVE = 1, + /// \brief Indoor heading. + HEADINGMODE_INDOOR = 2 }; /// \brief VPE modes for the VPE Basic Control register. -enum VpeMode -{ - /// \brief Off. - VPEMODE_OFF = 0, - /// \brief Mode 1. - VPEMODE_MODE1 = 1 +enum VpeMode { + /// \brief Off. + VPEMODE_OFF = 0, + /// \brief Mode 1. + VPEMODE_MODE1 = 1 }; /// \brief Different scenario modes for the INS Basic Configuration register. -enum Scenario -{ - /// \brief AHRS. - SCENARIO_AHRS = 0, - /// \brief General purpose INS with barometric pressure sensor. - SCENARIO_INSWITHPRESSURE = 1, - /// \brief General purpose INS without barometric pressure sensor. - SCENARIO_INSWITHOUTPRESSURE = 2, - /// \brief GPS moving baseline for dynamic applications. - SCENARIO_GPSMOVINGBASELINEDYNAMIC = 3, - /// \brief GPS moving baseline for static applications. - SCENARIO_GPSMOVINGBASELINESTATIC = 4 +enum Scenario { + /// \brief AHRS. + SCENARIO_AHRS = 0, + /// \brief General purpose INS with barometric pressure sensor. + SCENARIO_INSWITHPRESSURE = 1, + /// \brief General purpose INS without barometric pressure sensor. + SCENARIO_INSWITHOUTPRESSURE = 2, + /// \brief GPS moving baseline for dynamic applications. + SCENARIO_GPSMOVINGBASELINEDYNAMIC = 3, + /// \brief GPS moving baseline for static applications. + SCENARIO_GPSMOVINGBASELINESTATIC = 4 }; /// \brief HSI modes used for the Magnetometer Calibration Control register. -enum HsiMode -{ - /// \brief Real-time hard/soft iron calibration algorithm is turned off. - HSIMODE_OFF = 0, - /// \brief Runs the real-time hard/soft iron calibration algorithm. - HSIMODE_RUN = 1, - /// \brief Resets the real-time hard/soft iron solution. - HSIMODE_RESET = 2 +enum HsiMode { + /// \brief Real-time hard/soft iron calibration algorithm is turned off. + HSIMODE_OFF = 0, + /// \brief Runs the real-time hard/soft iron calibration algorithm. + HSIMODE_RUN = 1, + /// \brief Resets the real-time hard/soft iron solution. + HSIMODE_RESET = 2 }; /// \brief HSI output types for the Magnetometer Calibration Control register. -enum HsiOutput -{ - /// \brief Onboard HSI is not applied to the magnetic measurements. - HSIOUTPUT_NOONBOARD = 1, - /// \brief Onboard HSI is applied to the magnetic measurements. - HSIOUTPUT_USEONBOARD = 3 +enum HsiOutput { + /// \brief Onboard HSI is not applied to the magnetic measurements. + HSIOUTPUT_NOONBOARD = 1, + /// \brief Onboard HSI is applied to the magnetic measurements. + HSIOUTPUT_USEONBOARD = 3 }; /// \brief Type of velocity compensation performed by the VPE. -enum VelocityCompensationMode -{ - /// \brief Disabled - VELOCITYCOMPENSATIONMODE_DISABLED = 0, - /// \brief Body Measurement - VELOCITYCOMPENSATIONMODE_BODYMEASUREMENT = 1 +enum VelocityCompensationMode { + /// \brief Disabled + VELOCITYCOMPENSATIONMODE_DISABLED = 0, + /// \brief Body Measurement + VELOCITYCOMPENSATIONMODE_BODYMEASUREMENT = 1 }; /// \brief How the magnetometer is used by the filter. -enum MagneticMode -{ - /// \brief Magnetometer will only affect heading. - MAGNETICMODE_2D = 0, - /// \brief Magnetometer will affect heading, pitch, and roll. - MAGNETICMODE_3D = 1 +enum MagneticMode { + /// \brief Magnetometer will only affect heading. + MAGNETICMODE_2D = 0, + /// \brief Magnetometer will affect heading, pitch, and roll. + MAGNETICMODE_3D = 1 }; /// \brief Source for magnetometer used by the filter. -enum ExternalSensorMode -{ - /// \brief Use internal magnetometer. - EXTERNALSENSORMODE_INTERNAL = 0, - /// \brief Use external magnetometer. Will use measurement at every new time step. - EXTERNALSENSORMODE_EXTERNAL200HZ = 1, - /// \brief Use external magnetometer. Will only use when the measurement is updated. - EXTERNALSENSORMODE_EXTERNALONUPDATE = 2 +enum ExternalSensorMode { + /// \brief Use internal magnetometer. + EXTERNALSENSORMODE_INTERNAL = 0, + /// \brief Use external magnetometer. Will use measurement at every new time step. + EXTERNALSENSORMODE_EXTERNAL200HZ = 1, + /// \brief Use external magnetometer. Will only use when the measurement is updated. + EXTERNALSENSORMODE_EXTERNALONUPDATE = 2 }; /// \brief Options for the use of FOAM. -enum FoamInit -{ - /// \brief FOAM is not used. - FOAMINIT_NOFOAMINIT = 0, - /// \brief FOAM is used to initialize only pitch and roll. - FOAMINIT_FOAMINITPITCHROLL = 1, - /// \brief FOAM is used to initialize heading, pitch and roll. - FOAMINIT_FOAMINITHEADINGPITCHROLL = 2, - /// \brief FOAM is used to initialize pitch, roll and covariance. - FOAMINIT_FOAMINITPITCHROLLCOVARIANCE = 3, - /// \brief FOAM is used to initialize heading, pitch, roll and covariance - FOAMINIT_FOAMINITHEADINGPITCHROLLCOVARIANCE = 4 +enum FoamInit { + /// \brief FOAM is not used. + FOAMINIT_NOFOAMINIT = 0, + /// \brief FOAM is used to initialize only pitch and roll. + FOAMINIT_FOAMINITPITCHROLL = 1, + /// \brief FOAM is used to initialize heading, pitch and roll. + FOAMINIT_FOAMINITHEADINGPITCHROLL = 2, + /// \brief FOAM is used to initialize pitch, roll and covariance. + FOAMINIT_FOAMINITPITCHROLLCOVARIANCE = 3, + /// \brief FOAM is used to initialize heading, pitch, roll and covariance + FOAMINIT_FOAMINITHEADINGPITCHROLLCOVARIANCE = 4 }; /// \brief Sensor saturation flags. -enum SensSat -{ - SENSSAT_MAGX = 0x0001, ///< \brief Magnetometer X-axis is saturated. - SENSSAT_MAGY = 0x0002, ///< \brief Magnetometer Y-axis is saturated. - SENSSAT_MAGZ = 0x0004, ///< \brief Magnetometer Z-axis is saturated. - SENSSAT_ACCX = 0x0008, ///< \brief Accelerometer X-axis is saturated. - SENSSAT_ACCY = 0x0010, ///< \brief Accelerometer Y-axis is saturated. - SENSSAT_ACCZ = 0x0020, ///< \brief Accelerometer Z-axis is saturated. - SENSSAT_GYROX = 0x0040, ///< \brief Gyro X-axis is saturated. - SENSSAT_GYROY = 0x0080, ///< \brief Gyro Y-axis is saturated. - SENSSAT_GYROZ = 0x0100, ///< \brief Gyro Z-axis is saturated. - SENSSAT_PRES = 0x0200 ///< \brief Pressure measurement is saturated. +enum SensSat { + SENSSAT_MAGX = 0x0001, ///< \brief Magnetometer X-axis is saturated. + SENSSAT_MAGY = 0x0002, ///< \brief Magnetometer Y-axis is saturated. + SENSSAT_MAGZ = 0x0004, ///< \brief Magnetometer Z-axis is saturated. + SENSSAT_ACCX = 0x0008, ///< \brief Accelerometer X-axis is saturated. + SENSSAT_ACCY = 0x0010, ///< \brief Accelerometer Y-axis is saturated. + SENSSAT_ACCZ = 0x0020, ///< \brief Accelerometer Z-axis is saturated. + SENSSAT_GYROX = 0x0040, ///< \brief Gyro X-axis is saturated. + SENSSAT_GYROY = 0x0080, ///< \brief Gyro Y-axis is saturated. + SENSSAT_GYROZ = 0x0100, ///< \brief Gyro Z-axis is saturated. + SENSSAT_PRES = 0x0200 ///< \brief Pressure measurement is saturated. }; /// \brief Status indicators for VPE. struct VpeStatus { - /// \brief AttitudeQuality field. - uint8_t attitudeQuality; + /// \brief AttitudeQuality field. + uint8_t attitudeQuality; - /// \brief GyroSaturation field. - bool gyroSaturation; + /// \brief GyroSaturation field. + bool gyroSaturation; - /// \brief GyroSaturationRecovery field. - bool gyroSaturationRecovery; + /// \brief GyroSaturationRecovery field. + bool gyroSaturationRecovery; - /// \brief MagDistrubance field. - uint8_t magDisturbance; + /// \brief MagDistrubance field. + uint8_t magDisturbance; - /// \brief MagSaturation field. - bool magSaturation; + /// \brief MagSaturation field. + bool magSaturation; - /// \brief AccDisturbance field. - uint8_t accDisturbance; + /// \brief AccDisturbance field. + uint8_t accDisturbance; - /// \brief AccSaturation field. - bool accSaturation; + /// \brief AccSaturation field. + bool accSaturation; - /// \brief KnownMagDisturbance field. - bool knownMagDisturbance; + /// \brief KnownMagDisturbance field. + bool knownMagDisturbance; - /// \brief KnownAccelDisturbance field. - bool knownAccelDisturbance; + /// \brief KnownAccelDisturbance field. + bool knownAccelDisturbance; - /// \brief Default constructor. - VpeStatus(); + /// \brief Default constructor. + VpeStatus(); - /// \brief Constructs a VpeStatus from the raw bit field received - /// from the sensor. - explicit VpeStatus(uint16_t raw); + /// \brief Constructs a VpeStatus from the raw bit field received + /// from the sensor. + explicit VpeStatus(uint16_t raw); }; /// \brief Status flags for INS filters. -enum InsStatus -{ - INSSTATUS_NOT_TRACKING = 0x00, ///< \brief Not tracking. - INSSTATUS_SUFFICIENT_DYNAMIC_MOTION = 0x01, ///< \brief Sufficient dynamic motion. - INSSTATUS_TRACKING = 0x02, ///< \brief INS is tracking. - INSSTATUS_GPS_FIX = 0x04, ///< \brief Indicates proper GPS fix. - INSSTATUS_TIME_ERROR = 0x08, ///< \brief INS filter loop exceeds 5 ms. - INSSTATUS_IMU_ERROR = 0x10, ///< \brief IMU communication error. - INSSTATUS_MAG_PRES_ERROR = 0x20, ///< \brief Magnetometer or pressure sensor error. - INSSTATUS_GPS_ERROR = 0x40 ///< \brief GPS communication error. +enum InsStatus { + INSSTATUS_NOT_TRACKING = 0x00, ///< \brief Not tracking. + INSSTATUS_SUFFICIENT_DYNAMIC_MOTION = 0x01, ///< \brief Sufficient dynamic motion. + INSSTATUS_TRACKING = 0x02, ///< \brief INS is tracking. + INSSTATUS_GPS_FIX = 0x04, ///< \brief Indicates proper GPS fix. + INSSTATUS_TIME_ERROR = 0x08, ///< \brief INS filter loop exceeds 5 ms. + INSSTATUS_IMU_ERROR = 0x10, ///< \brief IMU communication error. + INSSTATUS_MAG_PRES_ERROR = 0x20, ///< \brief Magnetometer or pressure sensor error. + INSSTATUS_GPS_ERROR = 0x40 ///< \brief GPS communication error. }; /// \brief UTC time as represented by the VectorNav sensor. struct TimeUtc { - int8_t year; ///< \brief Year field. - uint8_t month; ///< \brief Month field. - uint8_t day; ///< \brief Day field. - uint8_t hour; ///< \brief Hour field. - uint8_t min; ///< \brief Min field. - uint8_t sec; ///< \brief Sec field. - uint16_t ms; ///< \brief Ms field. + int8_t year; ///< \brief Year field. + uint8_t month; ///< \brief Month field. + uint8_t day; ///< \brief Day field. + uint8_t hour; ///< \brief Hour field. + uint8_t min; ///< \brief Min field. + uint8_t sec; ///< \brief Sec field. + uint16_t ms; ///< \brief Ms field. - /// \brief Default constructor. - //TimeUtc() { } + /// \brief Default constructor. + //TimeUtc() { } }; /// \brief TimeInfo as represented by the VectorNav sensor. struct TimeInfo { - uint8_t timeStatus; ///< \brief Time Status field. - int8_t leapSecs; ///< \brief Leap Seconds field. + uint8_t timeStatus; ///< \brief Time Status field. + int8_t leapSecs; ///< \brief Leap Seconds field. }; /// \brief TimeInfo as represented by the VectorNav sensor. struct GnssDop { - float gDop; ///< \brief gDOP field. - float pDop; ///< \brief pDOP field. - float tDop; ///< \brief tDOP field. - float vDop; ///< \brief vDOP field. - float hDop; ///< \brief hDOP field. - float nDop; ///< \brief nDOP field. - float eDop; ///< \brief gDOP field. + float gDop; ///< \brief gDOP field. + float pDop; ///< \brief pDOP field. + float tDop; ///< \brief tDOP field. + float vDop; ///< \brief vDOP field. + float hDop; ///< \brief hDOP field. + float nDop; ///< \brief nDOP field. + float eDop; ///< \brief gDOP field. }; /// \brief Allows combining flags of the CommonGroup enum. @@ -698,8 +661,8 @@ AttitudeGroup operator|(AttitudeGroup lhs, AttitudeGroup rhs); /// \return The binary ORed value. InsGroup operator|(InsGroup lhs, InsGroup rhs); -} -} -} +} // namespace uart +} // namespace protocol +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/util.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/util.h index 701d0d8d..75a3992a 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/util.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/util.h @@ -1,13 +1,17 @@ #ifndef _VNPROTOCOL_UART_UTIL_H_ #define _VNPROTOCOL_UART_UTIL_H_ -#include #include +#include + #include "types.h" -namespace vn { -namespace protocol { -namespace uart { +namespace vn +{ +namespace protocol +{ +namespace uart +{ // Utility functions. @@ -23,7 +27,7 @@ std::string str(AsciiAsync val); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, AsciiAsync e); +std::ostream & operator<<(std::ostream & out, AsciiAsync e); /// \brief Converts a SensorError enum into a string. /// @@ -43,7 +47,7 @@ std::string str(BootloaderError val); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, SensorError e); +std::ostream & operator<<(std::ostream & out, SensorError e); /// \brief Converts a SyncInMode enum into a string. /// @@ -153,7 +157,6 @@ std::string str(GpsRate val); /// \return The converted value. std::string str(AntPower val); - /// \brief Converts a VpeEnable enum into a string. /// /// \param[in] val The VpeEnable enum value to convert to string. @@ -220,7 +223,7 @@ std::string str(FoamInit val); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, SyncInMode e); +std::ostream & operator<<(std::ostream & out, SyncInMode e); /// \brief Overloads the ostream << operator for easy usage in displaying /// SyncInEdge enums. @@ -228,7 +231,7 @@ std::ostream& operator<<(std::ostream& out, SyncInMode e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, SyncInEdge e); +std::ostream & operator<<(std::ostream & out, SyncInEdge e); /// \brief Overloads the ostream << operator for easy usage in displaying /// SyncOutMode enums. @@ -236,7 +239,7 @@ std::ostream& operator<<(std::ostream& out, SyncInEdge e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, SyncOutMode e); +std::ostream & operator<<(std::ostream & out, SyncOutMode e); /// \brief Overloads the ostream << operator for easy usage in displaying /// SyncOutPolarity enums. @@ -244,7 +247,7 @@ std::ostream& operator<<(std::ostream& out, SyncOutMode e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, SyncOutPolarity e); +std::ostream & operator<<(std::ostream & out, SyncOutPolarity e); /// \brief Overloads the ostream << operator for easy usage in displaying /// CountMode enums. @@ -252,7 +255,7 @@ std::ostream& operator<<(std::ostream& out, SyncOutPolarity e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, CountMode e); +std::ostream & operator<<(std::ostream & out, CountMode e); /// \brief Overloads the ostream << operator for easy usage in displaying /// StatusMode enums. @@ -260,7 +263,7 @@ std::ostream& operator<<(std::ostream& out, CountMode e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, StatusMode e); +std::ostream & operator<<(std::ostream & out, StatusMode e); /// \brief Overloads the ostream << operator for easy usage in displaying /// ChecksumMode enums. @@ -268,7 +271,7 @@ std::ostream& operator<<(std::ostream& out, StatusMode e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, ChecksumMode e); +std::ostream & operator<<(std::ostream & out, ChecksumMode e); /// \brief Overloads the ostream << operator for easy usage in displaying /// ErrorMode enums. @@ -276,7 +279,7 @@ std::ostream& operator<<(std::ostream& out, ChecksumMode e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, ErrorMode e); +std::ostream & operator<<(std::ostream & out, ErrorMode e); /// \brief Overloads the ostream << operator for easy usage in displaying /// FilterMode enums. @@ -284,7 +287,7 @@ std::ostream& operator<<(std::ostream& out, ErrorMode e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, FilterMode e); +std::ostream & operator<<(std::ostream & out, FilterMode e); /// \brief Overloads the ostream << operator for easy usage in displaying /// IntegrationFrame enums. @@ -292,7 +295,7 @@ std::ostream& operator<<(std::ostream& out, FilterMode e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, IntegrationFrame e); +std::ostream & operator<<(std::ostream & out, IntegrationFrame e); /// \brief Overloads the ostream << operator for easy usage in displaying /// CompensationMode enums. @@ -300,7 +303,7 @@ std::ostream& operator<<(std::ostream& out, IntegrationFrame e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, CompensationMode e); +std::ostream & operator<<(std::ostream & out, CompensationMode e); /// \brief Overloads the ostream << operator for easy usage in displaying /// AccCompensationMode enums. @@ -308,7 +311,7 @@ std::ostream& operator<<(std::ostream& out, CompensationMode e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, AccCompensationMode e); +std::ostream & operator<<(std::ostream & out, AccCompensationMode e); /// \brief Overloads the ostream << operator for easy usage in displaying /// EarthRateCorrection enums. @@ -316,7 +319,7 @@ std::ostream& operator<<(std::ostream& out, AccCompensationMode e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, EarthRateCorrection e); +std::ostream & operator<<(std::ostream & out, EarthRateCorrection e); /// \brief Overloads the ostream << operator for easy usage in displaying /// GpsFix enums. @@ -324,7 +327,7 @@ std::ostream& operator<<(std::ostream& out, EarthRateCorrection e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, GpsFix e); +std::ostream & operator<<(std::ostream & out, GpsFix e); /// \brief Overloads the ostream << operator for easy usage in displaying /// GpsMode enums. @@ -332,7 +335,7 @@ std::ostream& operator<<(std::ostream& out, GpsFix e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, GpsMode e); +std::ostream & operator<<(std::ostream & out, GpsMode e); /// \brief Overloads the ostream << operator for easy usage in displaying /// PpsSource enums. @@ -340,7 +343,7 @@ std::ostream& operator<<(std::ostream& out, GpsMode e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, PpsSource e); +std::ostream & operator<<(std::ostream & out, PpsSource e); /// \brief Overloads the ostream << operator for easy usage in displaying /// GpsRate enums. @@ -348,7 +351,7 @@ std::ostream& operator<<(std::ostream& out, PpsSource e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, GpsRate e); +std::ostream & operator<<(std::ostream & out, GpsRate e); /// \brief Overloads the ostream << operator for easy usage in displaying /// AntPower enums. @@ -356,7 +359,7 @@ std::ostream& operator<<(std::ostream& out, GpsRate e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, AntPower e); +std::ostream & operator<<(std::ostream & out, AntPower e); /// \brief Overloads the ostream << operator for easy usage in displaying /// VpeEnable enums. @@ -364,7 +367,7 @@ std::ostream& operator<<(std::ostream& out, AntPower e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, VpeEnable e); +std::ostream & operator<<(std::ostream & out, VpeEnable e); /// \brief Overloads the ostream << operator for easy usage in displaying /// HeadingMode enums. @@ -372,7 +375,7 @@ std::ostream& operator<<(std::ostream& out, VpeEnable e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, HeadingMode e); +std::ostream & operator<<(std::ostream & out, HeadingMode e); /// \brief Overloads the ostream << operator for easy usage in displaying /// VpeMode enums. @@ -380,7 +383,7 @@ std::ostream& operator<<(std::ostream& out, HeadingMode e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, VpeMode e); +std::ostream & operator<<(std::ostream & out, VpeMode e); /// \brief Overloads the ostream << operator for easy usage in displaying /// Scenario enums. @@ -388,7 +391,7 @@ std::ostream& operator<<(std::ostream& out, VpeMode e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, Scenario e); +std::ostream & operator<<(std::ostream & out, Scenario e); /// \brief Overloads the ostream << operator for easy usage in displaying /// HsiMode enums. @@ -396,7 +399,7 @@ std::ostream& operator<<(std::ostream& out, Scenario e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, HsiMode e); +std::ostream & operator<<(std::ostream & out, HsiMode e); /// \brief Overloads the ostream << operator for easy usage in displaying /// HsiOutput enums. @@ -404,7 +407,7 @@ std::ostream& operator<<(std::ostream& out, HsiMode e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, HsiOutput e); +std::ostream & operator<<(std::ostream & out, HsiOutput e); /// \brief Overloads the ostream << operator for easy usage in displaying /// VelocityCompensationMode enums. @@ -412,7 +415,7 @@ std::ostream& operator<<(std::ostream& out, HsiOutput e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, VelocityCompensationMode e); +std::ostream & operator<<(std::ostream & out, VelocityCompensationMode e); /// \brief Overloads the ostream << operator for easy usage in displaying /// MagneticMode enums. @@ -420,7 +423,7 @@ std::ostream& operator<<(std::ostream& out, VelocityCompensationMode e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, MagneticMode e); +std::ostream & operator<<(std::ostream & out, MagneticMode e); /// \brief Overloads the ostream << operator for easy usage in displaying /// ExternalSensorMode enums. @@ -428,7 +431,7 @@ std::ostream& operator<<(std::ostream& out, MagneticMode e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, ExternalSensorMode e); +std::ostream & operator<<(std::ostream & out, ExternalSensorMode e); /// \brief Overloads the ostream << operator for easy usage in displaying /// FoamInit enums. @@ -436,10 +439,10 @@ std::ostream& operator<<(std::ostream& out, ExternalSensorMode e); /// \param[in] out The ostream being output to. /// \param[in] e The enum to output to ostream. /// \return Reference to the current ostream. -std::ostream& operator<<(std::ostream& out, FoamInit e); +std::ostream & operator<<(std::ostream & out, FoamInit e); -} -} -} +} // namespace uart +} // namespace protocol +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/utilities.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/utilities.h index 65bd1863..ab019895 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/utilities.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/utilities.h @@ -6,55 +6,53 @@ #include #include -#include "int.h" #include "export.h" +#include "int.h" /// \brief Defines for the specific version of the VectorNav library. -#define VNAPI_MAJOR 1 -#define VNAPI_MINOR 2 -#define VNAPI_PATCH 0 -#define VNAPI_REVISION 126 +#define VNAPI_MAJOR 1 +#define VNAPI_MINOR 2 +#define VNAPI_PATCH 0 +#define VNAPI_REVISION 126 -namespace vn { +namespace vn +{ /// \brief Class for version information about the VectorNav library. class ApiVersion { - public: - - /// \brief Returns the major version of the VectorNav library. - /// - /// \return The major version. - static int major(); - - /// \brief Returns the minor version of the VectorNav library. - /// - /// \return The minor version - static int minor(); - - /// \brief Returns the patch version of the VectorNav library. - /// - /// \return The patch version. - static int patch(); - - /// \brief Returns the revision version of the VectorNav library. - /// - /// \return The revision version. - static int revision(); - - /// \brief Returns the full version of the VectorNav library. - /// - /// \return The library's version in a string format. - static std::string getVersion(); - + /// \brief Returns the major version of the VectorNav library. + /// + /// \return The major version. + static int major(); + + /// \brief Returns the minor version of the VectorNav library. + /// + /// \return The minor version + static int minor(); + + /// \brief Returns the patch version of the VectorNav library. + /// + /// \return The patch version. + static int patch(); + + /// \brief Returns the revision version of the VectorNav library. + /// + /// \return The revision version. + static int revision(); + + /// \brief Returns the full version of the VectorNav library. + /// + /// \return The library's version in a string format. + static std::string getVersion(); }; /// \brief Converts two characters encoded in hex to a uint8_t. /// /// \param[in] str Two characters string with hexadecimal encoding. /// \return The converted value. -uint8_t toUint8FromHexStr(const char* str); +uint8_t toUint8FromHexStr(const char * str); /// \brief Converts a 16-bit integer in sensor order to host order. /// @@ -80,10 +78,6 @@ uint64_t stoh(uint64_t sensorOrdered); /// \return The number of bits set. uint8_t countSetBits(uint8_t d); - - - - /// \brief Converts the character encoded as a hexadecimal to a uint8_t. /// /// \param[in] c The hexadecimal character to convert. @@ -94,14 +88,13 @@ uint8_t to_uint8_from_hexchar(char c); /// /// \param[in] str Two characters string with hexadecimal encoding. /// \return The converted value. -uint8_t to_uint8_from_hexstr(const char* str); +uint8_t to_uint8_from_hexstr(const char * str); /// \brief Converts four characters encoded in hex to a uint16_t. /// /// \param[in] str Four characters string with hexadecimal encoding. /// \return The converted value. -uint16_t to_uint16_from_hexstr(const char* str); - +uint16_t to_uint16_from_hexstr(const char * str); #if PYTHON @@ -117,12 +110,12 @@ uint16_t to_uint16_from_hexstr(const char* str); /// present. /// \return flag indicating succes so the user may choose to shut down /// gracefully should the DLL be missing -vn_proglib_DLLEXPORT bool checkDllValidity(const std::string& dllName, - const std::string& workingDirectory, - std::vector& missingDlls); +vn_proglib_DLLEXPORT bool checkDllValidity( + const std::string & dllName, const std::string & workingDirectory, + std::vector & missingDlls); #endif -} +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/vector.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/vector.h index 04d62474..cdb00cfb 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/vector.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/vector.h @@ -7,1194 +7,1038 @@ #define _VN_MATH_VECTOR_H_ #include -#include -#include #include +#include +#include #include "exceptions.h" #include "int.h" -namespace vn { -namespace math { +namespace vn +{ +namespace math +{ /// \brief Template for a Euclidean vector. template struct vec { - // Public Members ///////////////////////////////////////////////////////// + // Public Members ///////////////////////////////////////////////////////// public: + /// \brief The vector's components. + T c[tdim]; - /// \brief The vector's components. - T c[tdim]; - - // Constructors /////////////////////////////////////////////////////////// + // Constructors /////////////////////////////////////////////////////////// public: + /// \brief Creates a new vector with uninitialized components. + vec() {} - /// \brief Creates a new vector with uninitialized components. - vec() { } + /// \brief Creates new vector with components initialized to val. + /// + /// \param[in] val The initialization value. + explicit vec(T val) { std::fill_n(c, tdim, val); } - /// \brief Creates new vector with components initialized to val. - /// - /// \param[in] val The initialization value. - explicit vec(T val) - { - std::fill_n(c, tdim, val); - } - - // Helper Methods ///////////////////////////////////////////////////////// + // Helper Methods ///////////////////////////////////////////////////////// public: + /// \brief Vector with all of its components set to 0. + /// + /// \return The 0 vector. + static vec zero() { return vec(0); } - /// \brief Vector with all of its components set to 0. - /// - /// \return The 0 vector. - static vec zero() - { - return vec(0); - } - - /// \brief Vector with all of its components set to 1. - /// - /// \return The 1 vector. - static vec one() - { - return vec(1); - } + /// \brief Vector with all of its components set to 1. + /// + /// \return The 1 vector. + static vec one() { return vec(1); } - // Operator Overloads ///////////////////////////////////////////////////// + // Operator Overloads ///////////////////////////////////////////////////// public: - - /// \brief Indexing into the vector's components. - /// - /// \param[in] index 0-based component index. - /// \exception dimension_error The index exceeded the dimension of the vector. - T& operator[](size_t index) - { - return const_cast(static_cast(*this)[index]); - } - - /// \brief Indexing into the vector's components. - /// - /// \param[in] index 0-based component index. - /// \exception dimension_error The index exceeded the dimension of the vector. - const T& operator[](size_t index) const - { - assert(index < tdim); - - return c[index]; - } - - /// \brief Negates the vector. - /// - /// \return The negated vector. - vec operator-() const - { - return neg(); - } - - /// \brief Adds a vector to this vector. - /// - /// \param[in] rhs The right-side vector. - /// \return The resulting vector. - vec& operator+=(const vec& rhs) - { - for (size_t i = 0; i < tdim; i++) - c[i] += rhs.c[i]; - - return *this; - } - - /// \brief Subtracts a vector from this vector. - /// - /// \param[in] rhs The right-side vector. - /// \return The resulting vector. - vec& operator-=(const vec& rhs) - { - for (size_t i = 0; i < tdim; i++) - c[i] -= rhs.c[i]; - - return *this; - } - - /// \brief Multiplies the vector by a scalar. - /// - /// \param[in] rhs The scalar. - /// \return The multiplied vector. - vec& operator*=(const T& rhs) - { - for (size_t i = 0; i < tdim; i++) - c[i] *= rhs; - - return *this; - } - - /// \brief Divides the vector by a scalar. - /// - /// \param[in] rhs The scalar. - /// \return The divided vector. - vec& operator/=(const T & rhs) - { - for (size_t i = 0; i < tdim; i++) - c[i] /= rhs; - - return *this; - } - - // Public Methods ///////////////////////////////////////////////////////// + /// \brief Indexing into the vector's components. + /// + /// \param[in] index 0-based component index. + /// \exception dimension_error The index exceeded the dimension of the vector. + T & operator[](size_t index) { return const_cast(static_cast(*this)[index]); } + + /// \brief Indexing into the vector's components. + /// + /// \param[in] index 0-based component index. + /// \exception dimension_error The index exceeded the dimension of the vector. + const T & operator[](size_t index) const + { + assert(index < tdim); + + return c[index]; + } + + /// \brief Negates the vector. + /// + /// \return The negated vector. + vec operator-() const { return neg(); } + + /// \brief Adds a vector to this vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + vec & operator+=(const vec & rhs) + { + for (size_t i = 0; i < tdim; i++) c[i] += rhs.c[i]; + + return *this; + } + + /// \brief Subtracts a vector from this vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + vec & operator-=(const vec & rhs) + { + for (size_t i = 0; i < tdim; i++) c[i] -= rhs.c[i]; + + return *this; + } + + /// \brief Multiplies the vector by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The multiplied vector. + vec & operator*=(const T & rhs) + { + for (size_t i = 0; i < tdim; i++) c[i] *= rhs; + + return *this; + } + + /// \brief Divides the vector by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The divided vector. + vec & operator/=(const T & rhs) + { + for (size_t i = 0; i < tdim; i++) c[i] /= rhs; + + return *this; + } + + // Public Methods ///////////////////////////////////////////////////////// public: + /// \brief The vector's dimension. + /// + /// \return The vector's dimension. + size_t dim() const { return tdim; } + + /// \brief Negates the vector. + /// + /// \return The negated vector. + vec neg() const + { + vec v; + + for (size_t i = 0; i < tdim; i++) v.c[i] = -c[i]; + + return v; + } + + /// \brief The vector's magnitude. + /// + /// \return The magnitude. + T mag() const + { + T sumOfSquares = 0; + + for (size_t i = 0; i < tdim; i++) sumOfSquares += c[i] * c[i]; + + return sqrt(sumOfSquares); + } + + /// \brief Adds a vector to this vector. + /// + /// \param[in] toAdd The vector to add. + /// \return The resulting vector. + vec add(const vec & toAdd) const + { + vec v; + + for (size_t i = 0; i < tdim; i++) v.c[i] = c[i] + toAdd.c[i]; + + return v; + } + + /// \brief Subtracts a vector from this vector. + /// + /// \param[in] to_sub The vector to subtract from this. + /// \return The resulting vector. + vec sub(const vec & to_sub) const + { + vec v; + + for (size_t i = 0; i < tdim; i++) v.c[i] = c[i] - to_sub.c[i]; + + return v; + } + + /// \brief Multiplies the vector by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The multiplied vector. + vec mult(const double & scalar) const + { + vec v; + + for (size_t i = 0; i < tdim; i++) v.c[i] = c[i] * scalar; + + return v; + } + + /// \brief Divides the vector by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The divided vector. + vec div(const double & scalar) const + { + vec v; + + for (size_t i = 0; i < tdim; i++) v.c[i] = c[i] / scalar; + + return v; + } + + /// \brief Normalizes the vector. + /// + /// \return The normalized vector. + vec norm() const + { + vec v; + + T m = mag(); + + for (size_t i = 0; i < tdim; i++) v.c[i] = c[i] / m; + + return v; + } + + /// \brief Computes the dot product of this and the provided vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The computed dot product. + T dot(const vec & rhs) const + { + T runningSum = 0; + + for (size_t i = 0; i < tdim; i++) runningSum += c[i] * rhs.c[i]; - /// \brief The vector's dimension. - /// - /// \return The vector's dimension. - size_t dim() const { return tdim; } - - /// \brief Negates the vector. - /// - /// \return The negated vector. - vec neg() const - { - vec v; - - for (size_t i = 0; i < tdim; i++) - v.c[i] = -c[i]; - - return v; - } - - /// \brief The vector's magnitude. - /// - /// \return The magnitude. - T mag() const - { - T sumOfSquares = 0; - - for (size_t i = 0; i < tdim; i++) - sumOfSquares += c[i] * c[i]; - - return sqrt(sumOfSquares); - } - - /// \brief Adds a vector to this vector. - /// - /// \param[in] toAdd The vector to add. - /// \return The resulting vector. - vec add(const vec& toAdd) const - { - vec v; - - for (size_t i = 0; i < tdim; i++) - v.c[i] = c[i] + toAdd.c[i]; - - return v; - } - - /// \brief Subtracts a vector from this vector. - /// - /// \param[in] to_sub The vector to subtract from this. - /// \return The resulting vector. - vec sub(const vec& to_sub) const - { - vec v; - - for (size_t i = 0; i < tdim; i++) - v.c[i] = c[i] - to_sub.c[i]; - - return v; - } - - /// \brief Multiplies the vector by a scalar. - /// - /// \param[in] scalar The scalar value. - /// \return The multiplied vector. - vec mult(const double& scalar) const - { - vec v; - - for (size_t i = 0; i < tdim; i++) - v.c[i] = c[i] * scalar; - - return v; - } - - /// \brief Divides the vector by a scalar. - /// - /// \param[in] scalar The scalar value. - /// \return The divided vector. - vec div(const double& scalar) const - { - vec v; - - for (size_t i = 0; i < tdim; i++) - v.c[i] = c[i] / scalar; - - return v; - } - - /// \brief Normalizes the vector. - /// - /// \return The normalized vector. - vec norm() const - { - vec v; - - T m = mag(); - - for (size_t i = 0; i < tdim; i++) - v.c[i] = c[i] / m; - - return v; - } - - /// \brief Computes the dot product of this and the provided vector. - /// - /// \param[in] rhs The right-side vector. - /// \return The computed dot product. - T dot(const vec& rhs) const - { - T runningSum = 0; - - for (size_t i = 0; i < tdim; i++) - runningSum += c[i] * rhs.c[i]; - - return runningSum; - } - + return runningSum; + } }; // Specializations //////////////////////////////////////////////////////////// #if defined(_MSC_VER) - #pragma warning(push) - - // Disable warning about 'nonstandard extension used : nameless struct/union'. - #pragma warning(disable:4201) +#pragma warning(push) + +// Disable warning about 'nonstandard extension used : nameless struct/union'. +#pragma warning(disable : 4201) #endif /// \brief Vector with 2 component specialization. template struct vec<2, T> { - - // Public Members ///////////////////////////////////////////////////////// + // Public Members ///////////////////////////////////////////////////////// public: + union { + struct + { + /// \brief X (0-component). + T x; - union - { - struct - { - /// \brief X (0-component). - T x; + /// \brief Y (1-component). + T y; + }; - /// \brief Y (1-component). - T y; - }; + /// \brief The vector's components. + T c[2]; + }; - /// \brief The vector's components. - T c[2]; - }; + // Constructors /////////////////////////////////////////////////////////// - // Constructors /////////////////////////////////////////////////////////// +public: + /// \brief Creates a new vector with uninitialized components. + vec() {} + + /// \brief Creates new vector with components initialized to val. + /// + /// \param[in] val The initialization value. + explicit vec(T val) : x(val), y(val) {} + + /// \brief Creates a new vector with its components inintialized to the + /// provided values. + /// + /// \param[in] x_val The x value. + /// \param[in] y_val The y value. + vec(T x_val, T y_val) : x(x_val), y(y_val) {} + + // Helper Methods ///////////////////////////////////////////////////////// public: + /// \brief Vector with all of its components set to 0. + /// + /// \return The 0 vector. + static vec zero() { return vec<2, T>(0); } - /// \brief Creates a new vector with uninitialized components. - vec() { } + /// \brief Vector with all of its components set to 1. + /// + /// \return The 1 vector. + static vec one() { return vec<2, T>(1); } - /// \brief Creates new vector with components initialized to val. - /// - /// \param[in] val The initialization value. - explicit vec(T val) : x(val), y(val) { } + /// \brief Unit vector pointing in the X (0-component) direction. + /// + /// \return The unit vector. + static vec unitX() { return vec<2, T>(1, 0); } - /// \brief Creates a new vector with its components inintialized to the - /// provided values. - /// - /// \param[in] x_val The x value. - /// \param[in] y_val The y value. - vec(T x_val, T y_val) : x(x_val), y(y_val) { } + /// \brief Unit vector pointing in the Y (1-component) direction. + /// + /// \return The unit vector. + static vec unitY() { return vec<2, T>(0, 1); } - // Helper Methods ///////////////////////////////////////////////////////// + // Operator Overloads ///////////////////////////////////////////////////// public: - - /// \brief Vector with all of its components set to 0. - /// - /// \return The 0 vector. - static vec zero() - { - return vec<2, T>(0); - } - - /// \brief Vector with all of its components set to 1. - /// - /// \return The 1 vector. - static vec one() - { - return vec<2, T>(1); - } - - /// \brief Unit vector pointing in the X (0-component) direction. - /// - /// \return The unit vector. - static vec unitX() - { - return vec<2, T>(1, 0); - } - - /// \brief Unit vector pointing in the Y (1-component) direction. - /// - /// \return The unit vector. - static vec unitY() - { - return vec<2, T>(0, 1); - } - - - // Operator Overloads ///////////////////////////////////////////////////// + /// \brief Indexing into the vector's components. + /// + /// \param[in] index 0-based component index. + /// \exception dimension_error The index exceeded the dimension of the vector. + T & operator[](size_t index) { return const_cast(static_cast(*this)[index]); } + + /// \brief Indexing into the vector's components. + /// + /// \param[in] index 0-based component index. + /// \exception dimension_error The index exceeded the dimension of the vector. + const T & operator[](size_t index) const + { + assert(index < 2); + + return c[index]; + } + + /// \brief Negates the vector. + /// + /// \return The negated vector. + vec operator-() const { return neg(); } + + /// \brief Adds a vector to this vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + vec & operator+=(const vec & rhs) + { + x += rhs.x; + y += rhs.y; + + return *this; + } + + /// \brief Subtracts a vector from this vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + vec & operator-=(const vec & rhs) + { + x -= rhs.x; + y -= rhs.y; + + return *this; + } + + /// \brief Multiplies the vector by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The multiplied vector. + vec & operator*=(const T & rhs) + { + x *= rhs; + y *= rhs; + + return *this; + } + + /// \brief Divides the vector by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The divided vector. + vec & operator/=(const T & rhs) + { + x /= rhs; + y /= rhs; + + return *this; + } + + // Public Methods ///////////////////////////////////////////////////////// public: + /// \brief The vector's dimension. + /// + /// \return The vector's dimension. + size_t dim() const { return 2; } + + /// \brief Negates the vector. + /// + /// \return The negated vector. + vec neg() const + { +// TODO: Issue when the underlying type is an unsigned integer. +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4146) +#endif - /// \brief Indexing into the vector's components. - /// - /// \param[in] index 0-based component index. - /// \exception dimension_error The index exceeded the dimension of the vector. - T& operator[](size_t index) - { - return const_cast(static_cast(*this)[index]); - } - - /// \brief Indexing into the vector's components. - /// - /// \param[in] index 0-based component index. - /// \exception dimension_error The index exceeded the dimension of the vector. - const T& operator[](size_t index) const - { - assert(index < 2); - - return c[index]; - } - - /// \brief Negates the vector. - /// - /// \return The negated vector. - vec operator-() const - { - return neg(); - } - - /// \brief Adds a vector to this vector. - /// - /// \param[in] rhs The right-side vector. - /// \return The resulting vector. - vec& operator+=(const vec& rhs) - { - x += rhs.x; - y += rhs.y; - - return *this; - } - - /// \brief Subtracts a vector from this vector. - /// - /// \param[in] rhs The right-side vector. - /// \return The resulting vector. - vec& operator-=(const vec& rhs) - { - x -= rhs.x; - y -= rhs.y; - - return *this; - } - - /// \brief Multiplies the vector by a scalar. - /// - /// \param[in] rhs The scalar. - /// \return The multiplied vector. - vec& operator*=(const T& rhs) - { - x *= rhs; - y *= rhs; - - return *this; - } - - /// \brief Divides the vector by a scalar. - /// - /// \param[in] rhs The scalar. - /// \return The divided vector. - vec& operator/=(const T & rhs) - { - x /= rhs; - y /= rhs; - - return *this; - } - - // Public Methods ///////////////////////////////////////////////////////// + return vec(-x, -y); -public: +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + } + + /// \brief The vector's magnitude. + /// + /// \return The magnitude. + T mag() const + { +// TODO: Might want this method to return a float even if the underlying +// data type is integer. +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4244) +#endif - /// \brief The vector's dimension. - /// - /// \return The vector's dimension. - size_t dim() const { return 2; } - - /// \brief Negates the vector. - /// - /// \return The negated vector. - vec neg() const - { - // TODO: Issue when the underlying type is an unsigned integer. - #if defined(_MSC_VER) - #pragma warning(push) - #pragma warning(disable:4146) - #endif - - return vec(-x, -y); - - #if defined (_MSC_VER) - #pragma warning(pop) - #endif - } - - /// \brief The vector's magnitude. - /// - /// \return The magnitude. - T mag() const - { - // TODO: Might want this method to return a float even if the underlying - // data type is integer. - #if defined(_MSC_VER) - #pragma warning(push) - #pragma warning(disable:4244) - #endif - - #if (defined(_MSC_VER) && _MSC_VER <= 1600) - // HACK: Visual Studio 2010 has trouble determining the correct 'sqrt' - // function for the template int32_t. - return sqrt(static_cast(x*x + y*y)); - #else - // HACK: - return sqrt(static_cast(x*x + y*y)); - //return sqrt(x*x + y*y); - #endif - - #if defined (_MSC_VER) - #pragma warning(pop) - #endif - } - - /// \brief Adds a vector to this vector. - /// - /// \param[in] toAdd The vector to add. - /// \return The resulting vector. - vec add(const vec& toAdd) const - { - return vec(x + toAdd.x, y + toAdd.y); - } - - /// \brief Subtracts a vector from this vector. - /// - /// \param[in] to_sub The vector to subtract from this. - /// \return The resulting vector. - vec sub(const vec& to_sub) const - { - return vec(x - to_sub.x, y - to_sub.y); - } - - /// \brief Multiplies the vector by a scalar. - /// - /// \param[in] scalar The scalar value. - /// \return The multiplied vector. - vec mult(const double& scalar) const - { - return vec(x * scalar, y * scalar); - } - - /// \brief Divides the vector by a scalar. - /// - /// \param[in] scalar The scalar value. - /// \return The divided vector. - vec div(const double& scalar) const - { - return vec(x / scalar, y / scalar); - } - - /// \brief Normalizes the vector. - /// - /// \return The normalized vector. - vec norm() const - { - T m = mag(); - - return vec(x / m, y / m); - } - - /// \brief Computes the dot product of this and the provided vector. - /// - /// \param[in] rhs The right-side vector. - /// \return The computed dot product. - T dot(const vec& rhs) const - { - return x*rhs.x + y*rhs.y; - } +#if (defined(_MSC_VER) && _MSC_VER <= 1600) + // HACK: Visual Studio 2010 has trouble determining the correct 'sqrt' + // function for the template int32_t. + return sqrt(static_cast(x * x + y * y)); +#else + // HACK: + return sqrt(static_cast(x * x + y * y)); +//return sqrt(x*x + y*y); +#endif +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + } + + /// \brief Adds a vector to this vector. + /// + /// \param[in] toAdd The vector to add. + /// \return The resulting vector. + vec add(const vec & toAdd) const { return vec(x + toAdd.x, y + toAdd.y); } + + /// \brief Subtracts a vector from this vector. + /// + /// \param[in] to_sub The vector to subtract from this. + /// \return The resulting vector. + vec sub(const vec & to_sub) const { return vec(x - to_sub.x, y - to_sub.y); } + + /// \brief Multiplies the vector by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The multiplied vector. + vec mult(const double & scalar) const { return vec(x * scalar, y * scalar); } + + /// \brief Divides the vector by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The divided vector. + vec div(const double & scalar) const { return vec(x / scalar, y / scalar); } + + /// \brief Normalizes the vector. + /// + /// \return The normalized vector. + vec norm() const + { + T m = mag(); + + return vec(x / m, y / m); + } + + /// \brief Computes the dot product of this and the provided vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The computed dot product. + T dot(const vec & rhs) const { return x * rhs.x + y * rhs.y; } }; - /// \brief Vector with 3 component specialization. template struct vec<3, T> { - - // Public Members ///////////////////////////////////////////////////////// + // Public Members ///////////////////////////////////////////////////////// public: + union { + struct + { + /// \brief X (0-component). + T x; - union - { - struct - { - /// \brief X (0-component). - T x; + /// \brief Y (1-component). + T y; - /// \brief Y (1-component). - T y; + /// \brief Z (2-component). + T z; + }; - /// \brief Z (2-component). - T z; - }; + struct + { + /// \brief Red (0-component). + T r; - struct - { - /// \brief Red (0-component). - T r; + /// \brief Green (1-component). + T g; - /// \brief Green (1-component). - T g; + /// \brief Blue (2-component). + T b; + }; - /// \brief Blue (2-component). - T b; - }; +// Union of template class with constructor not allowed until C++11. +#if __cplusplus >= 201103L - // Union of template class with constructor not allowed until C++11. - #if __cplusplus >= 201103L + /// \brief XY (0,1-components). + vec<2, T> xy; - /// \brief XY (0,1-components). - vec<2, T> xy; +#endif - #endif - - /// \brief The vector's components. - T c[3]; - }; + /// \brief The vector's components. + T c[3]; + }; - // Constructors /////////////////////////////////////////////////////////// + // Constructors /////////////////////////////////////////////////////////// public: + /// \brief Creates a new vector with uninitialized components. + vec() {} - /// \brief Creates a new vector with uninitialized components. - vec() { } - - /// \brief Creates new vector with components initialized to val. - /// - /// \param[in] val The initialization value. - explicit vec(T val) : x(val), y(val), z(val) { } + /// \brief Creates new vector with components initialized to val. + /// + /// \param[in] val The initialization value. + explicit vec(T val) : x(val), y(val), z(val) {} - /// \brief Creates a new vector with its components initialized to the - /// provided values. - /// - /// \param[in] x_val The x value. - /// \param[in] y_val The y value. - /// \param[in] z_val The z value. - vec(const T& x_val, const T& y_val, const T& z_val) : x(x_val), y(y_val), z(z_val) { } + /// \brief Creates a new vector with its components initialized to the + /// provided values. + /// + /// \param[in] x_val The x value. + /// \param[in] y_val The y value. + /// \param[in] z_val The z value. + vec(const T & x_val, const T & y_val, const T & z_val) : x(x_val), y(y_val), z(z_val) {} - // Helper Methods ///////////////////////////////////////////////////////// + // Helper Methods ///////////////////////////////////////////////////////// public: + /// \brief Vector with all of its components set to 0. + /// + /// \return The 0 vector. + static vec zero() { return vec<3, T>(0); } - /// \brief Vector with all of its components set to 0. - /// - /// \return The 0 vector. - static vec zero() - { - return vec<3, T>(0); - } - - /// \brief Vector with all of its components set to 1. - /// - /// \return The 1 vector. - static vec one() - { - return vec<3, T>(1); - } - - /// \brief Unit vector pointing in the X (0-component) direction. - /// - /// \return The unit vector. - static vec unitX() - { - return vec<3, T>(1, 0, 0); - } - - /// \brief Unit vector pointing in the Y (1-component) direction. - /// - /// \return The unit vector. - static vec unitY() - { - return vec<3, T>(0, 1, 0); - } - - /// \brief Unit vector pointing in the Z (2-component) direction. - /// - /// \return The unit vector. - static vec unitZ() - { - return vec<3, T>(0, 0, 1); - } - - // Operator Overloads ///////////////////////////////////////////////////// + /// \brief Vector with all of its components set to 1. + /// + /// \return The 1 vector. + static vec one() { return vec<3, T>(1); } -public: + /// \brief Unit vector pointing in the X (0-component) direction. + /// + /// \return The unit vector. + static vec unitX() { return vec<3, T>(1, 0, 0); } + + /// \brief Unit vector pointing in the Y (1-component) direction. + /// + /// \return The unit vector. + static vec unitY() { return vec<3, T>(0, 1, 0); } + + /// \brief Unit vector pointing in the Z (2-component) direction. + /// + /// \return The unit vector. + static vec unitZ() { return vec<3, T>(0, 0, 1); } + + // Operator Overloads ///////////////////////////////////////////////////// - /// \brief Indexing into the vector's components. - /// - /// \param[in] index 0-based component index. - /// \exception dimension_error The index exceeded the dimension of the vector. - T& operator[](size_t index) - { - return const_cast(static_cast(*this)[index]); - } - - /// \brief Indexing into the vector's components. - /// - /// \param[in] index 0-based component index. - /// \exception dimension_error The index exceeded the dimension of the vector. - const T& operator[](size_t index) const - { - assert(index < 3); - - return c[index]; - } - - /// \brief Negates the vector. - /// - /// \return The negated vector. - vec operator-() const - { - return neg(); - } - - /// \brief Adds a vector to this vector. - /// - /// \param[in] rhs The right-side vector. - /// \return The resulting vector. - vec& operator+=(const vec& rhs) - { - for (size_t i = 0; i < 3; i++) - c[i] += rhs.c[i]; - - return *this; - } - - /// \brief Subtracts a vector from this vector. - /// - /// \param[in] rhs The right-side vector. - /// \return The resulting vector. - vec& operator-=(const vec& rhs) - { - for (size_t i = 0; i < 3; i++) - c[i] -= rhs.c[i]; - - return *this; - } - - /// \brief Multiplies the vector by a scalar. - /// - /// \param[in] rhs The scalar. - /// \return The multiplied vector. - vec& operator*=(const T& rhs) - { - for (size_t i = 0; i < 3; i++) - c[i] *= rhs; - - return *this; - } - - /// \brief Divides the vector by a scalar. - /// - /// \param[in] rhs The scalar. - /// \return The divided vector. - vec& operator/=(const T & rhs) - { - for (size_t i = 0; i < 3; i++) - c[i] /= rhs; - - return *this; - } - - // Public Methods ///////////////////////////////////////////////////////// +public: + /// \brief Indexing into the vector's components. + /// + /// \param[in] index 0-based component index. + /// \exception dimension_error The index exceeded the dimension of the vector. + T & operator[](size_t index) { return const_cast(static_cast(*this)[index]); } + + /// \brief Indexing into the vector's components. + /// + /// \param[in] index 0-based component index. + /// \exception dimension_error The index exceeded the dimension of the vector. + const T & operator[](size_t index) const + { + assert(index < 3); + + return c[index]; + } + + /// \brief Negates the vector. + /// + /// \return The negated vector. + vec operator-() const { return neg(); } + + /// \brief Adds a vector to this vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + vec & operator+=(const vec & rhs) + { + for (size_t i = 0; i < 3; i++) c[i] += rhs.c[i]; + + return *this; + } + + /// \brief Subtracts a vector from this vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + vec & operator-=(const vec & rhs) + { + for (size_t i = 0; i < 3; i++) c[i] -= rhs.c[i]; + + return *this; + } + + /// \brief Multiplies the vector by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The multiplied vector. + vec & operator*=(const T & rhs) + { + for (size_t i = 0; i < 3; i++) c[i] *= rhs; + + return *this; + } + + /// \brief Divides the vector by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The divided vector. + vec & operator/=(const T & rhs) + { + for (size_t i = 0; i < 3; i++) c[i] /= rhs; + + return *this; + } + + // Public Methods ///////////////////////////////////////////////////////// public: + /// \brief The vector's dimension. + /// + /// \return The vector's dimension. + size_t dim() const { return 3; } + + /// \brief Negates the vector. + /// + /// \return The negated vector. + vec neg() const + { +// TODO: Issue when the underlying type is an unsigned integer. +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4146) +#endif + + return vec(-x, -y, -z); + +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + } + + /// \brief The vector's magnitude. + /// + /// \return The magnitude. + T mag() const + { +// TODO: Might want this method to return a float even if the underlying +// data type is integer. +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4244) +#endif - /// \brief The vector's dimension. - /// - /// \return The vector's dimension. - size_t dim() const { return 3; } - - /// \brief Negates the vector. - /// - /// \return The negated vector. - vec neg() const - { - // TODO: Issue when the underlying type is an unsigned integer. - #if defined(_MSC_VER) - #pragma warning(push) - #pragma warning(disable:4146) - #endif - - return vec(-x, -y, -z); - - #if defined (_MSC_VER) - #pragma warning(pop) - #endif - } - - /// \brief The vector's magnitude. - /// - /// \return The magnitude. - T mag() const - { - // TODO: Might want this method to return a float even if the underlying - // data type is integer. - #if defined(_MSC_VER) - #pragma warning(push) - #pragma warning(disable:4244) - #endif - - #if (defined(_MSC_VER) && _MSC_VER <= 1600) - // HACK: Visual Studio 2010 has trouble determining the correct 'sqrt' - // function for the template int32_t. - return sqrt(static_cast(x*x + y*y + z*z)); - #else - // HACK: - return sqrt(static_cast(x*x + y*y + z*z)); - //return sqrt(x*x + y*y + z*z); - #endif - - #if defined (_MSC_VER) - #pragma warning(pop) - #endif - } - - /// \brief Adds a vector to this vector. - /// - /// \param[in] toAdd The vector to add. - /// \return The resulting vector. - vec add(const vec& toAdd) const - { - return vec(x + toAdd.x, y + toAdd.y, z + toAdd.z); - } - - /// \brief Subtracts a vector from this vector. - /// - /// \param[in] to_sub The vector to subtract from this. - /// \return The resulting vector. - vec sub(const vec& to_sub) const - { - return vec(x - to_sub.x, y - to_sub.y, z - to_sub.z); - } - - /// \brief Multiplies the vector by a scalar. - /// - /// \param[in] scalar The scalar value. - /// \return The multiplied vector. - vec mult(const double& scalar) const - { - return vec(x * scalar, y * scalar, z * scalar); - } - - /// \brief Divides the vector by a scalar. - /// - /// \param[in] scalar The scalar value. - /// \return The divided vector. - vec div(const double& scalar) const - { - return vec(x / scalar, y / scalar, z / scalar); - } - - /// \brief Normalizes the vector. - /// - /// \return The normalized vector. - vec norm() const - { - T m = mag(); - - return vec(x / m, y / m, z / m); - } - - /// \brief Computes the dot product of this and the provided vector. - /// - /// \param[in] rhs The right-side vector. - /// \return The computed dot product. - T dot(const vec& rhs) const - { - return x*rhs.x + y*rhs.y + z*rhs.z; - } - - /// \brief Computes the cross product of this and the provided vector. - /// - /// \param[in] rhs The right-side vector. - /// \return The computed cross product. - /// \exception dimension_error The dimension of the vector is not 3. - vec<3, T> cross(const vec<3, T>& rhs) const - { - vec<3, T> v; - - v.c[0] = c[1] * rhs.c[2] - c[2] * rhs.c[1]; - v.c[1] = c[2] * rhs.c[0] - c[0] * rhs.c[2]; - v.c[2] = c[0] * rhs.c[1] - c[1] * rhs.c[0]; - - return v; - } +#if (defined(_MSC_VER) && _MSC_VER <= 1600) + // HACK: Visual Studio 2010 has trouble determining the correct 'sqrt' + // function for the template int32_t. + return sqrt(static_cast(x * x + y * y + z * z)); +#else + // HACK: + return sqrt(static_cast(x * x + y * y + z * z)); +//return sqrt(x*x + y*y + z*z); +#endif +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + } + + /// \brief Adds a vector to this vector. + /// + /// \param[in] toAdd The vector to add. + /// \return The resulting vector. + vec add(const vec & toAdd) const { return vec(x + toAdd.x, y + toAdd.y, z + toAdd.z); } + + /// \brief Subtracts a vector from this vector. + /// + /// \param[in] to_sub The vector to subtract from this. + /// \return The resulting vector. + vec sub(const vec & to_sub) const { return vec(x - to_sub.x, y - to_sub.y, z - to_sub.z); } + + /// \brief Multiplies the vector by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The multiplied vector. + vec mult(const double & scalar) const { return vec(x * scalar, y * scalar, z * scalar); } + + /// \brief Divides the vector by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The divided vector. + vec div(const double & scalar) const { return vec(x / scalar, y / scalar, z / scalar); } + + /// \brief Normalizes the vector. + /// + /// \return The normalized vector. + vec norm() const + { + T m = mag(); + + return vec(x / m, y / m, z / m); + } + + /// \brief Computes the dot product of this and the provided vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The computed dot product. + T dot(const vec & rhs) const { return x * rhs.x + y * rhs.y + z * rhs.z; } + + /// \brief Computes the cross product of this and the provided vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The computed cross product. + /// \exception dimension_error The dimension of the vector is not 3. + vec<3, T> cross(const vec<3, T> & rhs) const + { + vec<3, T> v; + + v.c[0] = c[1] * rhs.c[2] - c[2] * rhs.c[1]; + v.c[1] = c[2] * rhs.c[0] - c[0] * rhs.c[2]; + v.c[2] = c[0] * rhs.c[1] - c[1] * rhs.c[0]; + + return v; + } }; /// \brief Vector with 4 component specialization. template struct vec<4, T> { - - // Public Members ///////////////////////////////////////////////////////// + // Public Members ///////////////////////////////////////////////////////// public: + union { + struct + { + /// \brief X (0-component). + T x; - union - { - struct - { - /// \brief X (0-component). - T x; - - /// \brief Y (1-component). - T y; + /// \brief Y (1-component). + T y; - /// \brief Z (2-component). - T z; + /// \brief Z (2-component). + T z; - /// \brief W (3-component). - T w; - }; + /// \brief W (3-component). + T w; + }; - struct - { - /// \brief Red (0-component). - T r; + struct + { + /// \brief Red (0-component). + T r; - /// \brief Green (1-component). - T g; + /// \brief Green (1-component). + T g; - /// \brief Blue (2-component). - T b; + /// \brief Blue (2-component). + T b; - /// \brief Alpha (3-component). - T a; - }; + /// \brief Alpha (3-component). + T a; + }; - // Union of template class with constructor not allowed until C++11. - #if __cplusplus >= 201103L +// Union of template class with constructor not allowed until C++11. +#if __cplusplus >= 201103L - /// \brief XY (0,1-components). - vec<2, T> xy; + /// \brief XY (0,1-components). + vec<2, T> xy; - /// \brief XYZ (0,1,2-components). - vec<3, T> xyz; + /// \brief XYZ (0,1,2-components). + vec<3, T> xyz; - /// \brief RGB (0,1,2-components). - vec<3, T> rgb; + /// \brief RGB (0,1,2-components). + vec<3, T> rgb; - #endif +#endif - /// \brief The vector's components. - T c[4]; - }; + /// \brief The vector's components. + T c[4]; + }; - // Constructors /////////////////////////////////////////////////////////// + // Constructors /////////////////////////////////////////////////////////// public: + /// \brief Creates a new vector with uninitialized components. + vec() {} - /// \brief Creates a new vector with uninitialized components. - vec() { } - - /// \brief Creates new vector with components initialized to val. - /// - /// \param[in] val The initialization value. - explicit vec(T val) : x(val), y(val), z(val), w(val) { } + /// \brief Creates new vector with components initialized to val. + /// + /// \param[in] val The initialization value. + explicit vec(T val) : x(val), y(val), z(val), w(val) {} - /// \brief Creates a new vector with its components inintialized to the - /// provided values. - /// - /// \param[in] x_val The x value. - /// \param[in] y_val The y value. - /// \param[in] z_val The z value. - /// \param[in] w_val The w value. - vec(T x_val, T y_val, T z_val, T w_val) : x(x_val), y(y_val), z(z_val), w(w_val) { } + /// \brief Creates a new vector with its components inintialized to the + /// provided values. + /// + /// \param[in] x_val The x value. + /// \param[in] y_val The y value. + /// \param[in] z_val The z value. + /// \param[in] w_val The w value. + vec(T x_val, T y_val, T z_val, T w_val) : x(x_val), y(y_val), z(z_val), w(w_val) {} - // Helper Methods ///////////////////////////////////////////////////////// + // Helper Methods ///////////////////////////////////////////////////////// public: + /// \brief Vector with all of its components set to 0. + /// + /// \return The 0 vector. + static vec zero() { return vec<4, T>(0); } + + /// \brief Vector with all of its components set to 1. + /// + /// \return The 1 vector. + static vec one() { return vec<4, T>(1); } + + /// \brief Unit vector pointing in the X (0-component) direction. + /// + /// \return The unit vector. + static vec unitX() { return vec<4, T>(1, 0, 0, 0); } + + /// \brief Unit vector pointing in the Y (1-component) direction. + /// + /// \return The unit vector. + static vec unitY() { return vec<4, T>(0, 1, 0, 0); } + + /// \brief Unit vector pointing in the Z (2-component) direction. + /// + /// \return The unit vector. + static vec unitZ() { return vec<4, T>(0, 0, 1, 0); } + + /// \brief Unit vector pointing in the W (3-component) direction. + /// + /// \return The unit vector. + static vec unitW() { return vec<4, T>(0, 0, 0, 1); } + // Operator Overloads ///////////////////////////////////////////////////// - /// \brief Vector with all of its components set to 0. - /// - /// \return The 0 vector. - static vec zero() - { - return vec<4, T>(0); - } - - /// \brief Vector with all of its components set to 1. - /// - /// \return The 1 vector. - static vec one() - { - return vec<4, T>(1); - } - - /// \brief Unit vector pointing in the X (0-component) direction. - /// - /// \return The unit vector. - static vec unitX() - { - return vec<4, T>(1, 0, 0, 0); - } - - /// \brief Unit vector pointing in the Y (1-component) direction. - /// - /// \return The unit vector. - static vec unitY() - { - return vec<4, T>(0, 1, 0, 0); - } - - /// \brief Unit vector pointing in the Z (2-component) direction. - /// - /// \return The unit vector. - static vec unitZ() - { - return vec<4, T>(0, 0, 1, 0); - } - - /// \brief Unit vector pointing in the W (3-component) direction. - /// - /// \return The unit vector. - static vec unitW() - { - return vec<4, T>(0, 0, 0, 1); - } - // Operator Overloads ///////////////////////////////////////////////////// +public: + /// \brief Indexing into the vector's components. + /// + /// \param[in] index 0-based component index. + /// \exception dimension_error The index exceeded the dimension of the vector. + T & operator[](size_t index) { return const_cast(static_cast(*this)[index]); } + + /// \brief Indexing into the vector's components. + /// + /// \param[in] index 0-based component index. + /// \exception dimension_error The index exceeded the dimension of the vector. + const T & operator[](size_t index) const + { + assert(index < 4); + + return c[index]; + } + + /// \brief Negates the vector. + /// + /// \return The negated vector. + vec operator-() const { return neg(); } + + /// \brief Adds a vector to this vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + vec & operator+=(const vec & rhs) + { + x += rhs.x; + y += rhs.y; + z += rhs.z; + w += rhs.w; + + return *this; + } + + /// \brief Subtracts a vector from this vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + vec & operator-=(const vec & rhs) + { + x -= rhs.x; + y -= rhs.y; + z -= rhs.z; + w -= rhs.w; + + return *this; + } + + /// \brief Multiplies the vector by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The multiplied vector. + vec & operator*=(const T & rhs) + { + x *= rhs; + y *= rhs; + z *= rhs; + w *= rhs; + + return *this; + } + + /// \brief Divides the vector by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The divided vector. + vec & operator/=(const T & rhs) + { + x /= rhs; + y /= rhs; + z /= rhs; + w /= rhs; + + return *this; + } + + // Public Methods ///////////////////////////////////////////////////////// public: + /// \brief The vector's dimension. + /// + /// \return The vector's dimension. + size_t dim() const { return 4; } + + /// \brief Negates the vector. + /// + /// \return The negated vector. + vec neg() const + { +// TODO: Issue when the underlying type is an unsigned integer. +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4146) +#endif - /// \brief Indexing into the vector's components. - /// - /// \param[in] index 0-based component index. - /// \exception dimension_error The index exceeded the dimension of the vector. - T& operator[](size_t index) - { - return const_cast(static_cast(*this)[index]); - } - - /// \brief Indexing into the vector's components. - /// - /// \param[in] index 0-based component index. - /// \exception dimension_error The index exceeded the dimension of the vector. - const T& operator[](size_t index) const - { - assert(index < 4); - - return c[index]; - } - - /// \brief Negates the vector. - /// - /// \return The negated vector. - vec operator-() const - { - return neg(); - } - - /// \brief Adds a vector to this vector. - /// - /// \param[in] rhs The right-side vector. - /// \return The resulting vector. - vec& operator+=(const vec& rhs) - { - x += rhs.x; - y += rhs.y; - z += rhs.z; - w += rhs.w; - - return *this; - } - - /// \brief Subtracts a vector from this vector. - /// - /// \param[in] rhs The right-side vector. - /// \return The resulting vector. - vec& operator-=(const vec& rhs) - { - x -= rhs.x; - y -= rhs.y; - z -= rhs.z; - w -= rhs.w; - - return *this; - } - - /// \brief Multiplies the vector by a scalar. - /// - /// \param[in] rhs The scalar. - /// \return The multiplied vector. - vec& operator*=(const T& rhs) - { - x *= rhs; - y *= rhs; - z *= rhs; - w *= rhs; - - return *this; - } - - /// \brief Divides the vector by a scalar. - /// - /// \param[in] rhs The scalar. - /// \return The divided vector. - vec& operator/=(const T & rhs) - { - x /= rhs; - y /= rhs; - z /= rhs; - w /= rhs; - - return *this; - } - - // Public Methods ///////////////////////////////////////////////////////// + return vec(-x, -y, -z, -w); -public: +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + } + + /// \brief The vector's magnitude. + /// + /// \return The magnitude. + T mag() const + { +// TODO: Might want this method to return a float even if the underlying +// data type is integer. +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4244) +#endif + +#if (defined(_MSC_VER) && _MSC_VER <= 1600) + // HACK: Visual Studio 2010 has trouble determining the correct 'sqrt' + // function for the template int32_t. + return sqrt(static_cast(x * x + y * y + z * z + w * w)); +#else + // HACK: + return sqrt(static_cast(x * x + y * y + z * z + w * w)); +//return sqrt(x*x + y*y + z*z + w*w); +#endif - /// \brief The vector's dimension. - /// - /// \return The vector's dimension. - size_t dim() const { return 4; } - - /// \brief Negates the vector. - /// - /// \return The negated vector. - vec neg() const - { - // TODO: Issue when the underlying type is an unsigned integer. - #if defined(_MSC_VER) - #pragma warning(push) - #pragma warning(disable:4146) - #endif - - return vec(-x, -y, -z, -w); - - #if defined (_MSC_VER) - #pragma warning(pop) - #endif - } - - /// \brief The vector's magnitude. - /// - /// \return The magnitude. - T mag() const - { - // TODO: Might want this method to return a float even if the underlying - // data type is integer. - #if defined(_MSC_VER) - #pragma warning(push) - #pragma warning(disable:4244) - #endif - - #if (defined(_MSC_VER) && _MSC_VER <= 1600) - // HACK: Visual Studio 2010 has trouble determining the correct 'sqrt' - // function for the template int32_t. - return sqrt(static_cast(x*x + y*y + z*z + w*w)); - #else - // HACK: - return sqrt(static_cast(x*x + y*y + z*z + w*w)); - //return sqrt(x*x + y*y + z*z + w*w); - #endif - - #if defined (_MSC_VER) - #pragma warning(pop) - #endif - } - - /// \brief Adds a vector to this vector. - /// - /// \param[in] toAdd The vector to add. - /// \return The resulting vector. - vec add(const vec& toAdd) const - { - return vec(x + toAdd.x, y + toAdd.y, z + toAdd.z, w + toAdd.w); - } - - /// \brief Subtracts a vector from this vector. - /// - /// \param[in] to_sub The vector to subtract from this. - /// \return The resulting vector. - vec sub(const vec& to_sub) const - { - return vec(x - to_sub.x, y - to_sub.y, z - to_sub.z, w - to_sub.w); - } - - /// \brief Multiplies the vector by a scalar. - /// - /// \param[in] scalar The scalar value. - /// \return The multiplied vector. - vec mult(const double& scalar) const - { - return vec(x * scalar, y * scalar, z * scalar, w * scalar); - } - - /// \brief Divides the vector by a scalar. - /// - /// \param[in] scalar The scalar value. - /// \return The divided vector. - vec div(const double& scalar) const - { - return vec(x / scalar, y / scalar, z / scalar, w / scalar); - } - - /// \brief Normalizes the vector. - /// - /// \return The normalized vector. - vec norm() const - { - T m = mag(); - - return vec(x / m, y / m, z / m, w / m); - } - - /// \brief Computes the dot product of this and the provided vector. - /// - /// \param[in] rhs The right-side vector. - /// \return The computed dot product. - T dot(const vec& rhs) const - { - return x*rhs.x + y*rhs.y + z*rhs.z + w*rhs.w; - } +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + } + + /// \brief Adds a vector to this vector. + /// + /// \param[in] toAdd The vector to add. + /// \return The resulting vector. + vec add(const vec & toAdd) const + { + return vec(x + toAdd.x, y + toAdd.y, z + toAdd.z, w + toAdd.w); + } + + /// \brief Subtracts a vector from this vector. + /// + /// \param[in] to_sub The vector to subtract from this. + /// \return The resulting vector. + vec sub(const vec & to_sub) const + { + return vec(x - to_sub.x, y - to_sub.y, z - to_sub.z, w - to_sub.w); + } + + /// \brief Multiplies the vector by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The multiplied vector. + vec mult(const double & scalar) const + { + return vec(x * scalar, y * scalar, z * scalar, w * scalar); + } + + /// \brief Divides the vector by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The divided vector. + vec div(const double & scalar) const + { + return vec(x / scalar, y / scalar, z / scalar, w / scalar); + } + + /// \brief Normalizes the vector. + /// + /// \return The normalized vector. + vec norm() const + { + T m = mag(); + + return vec(x / m, y / m, z / m, w / m); + } + + /// \brief Computes the dot product of this and the provided vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The computed dot product. + T dot(const vec & rhs) const { return x * rhs.x + y * rhs.y + z * rhs.z + w * rhs.w; } }; -#if defined (_MSC_VER) - #pragma warning(pop) +#if defined(_MSC_VER) +#pragma warning(pop) #endif // Operator Overloads ///////////////////////////////////////////////////////// @@ -1205,11 +1049,11 @@ struct vec<4, T> /// \param[in] rhs The right-side vector. /// \return The resulting vector. template -vec operator+(vec lhs, const vec& rhs) +vec operator+(vec lhs, const vec & rhs) { - lhs += rhs; + lhs += rhs; - return lhs; + return lhs; } /// \brief Subtracts a vector from another vector. @@ -1218,18 +1062,18 @@ vec operator+(vec lhs, const vec& rhs) /// \param[in] rhs The right-side vector. /// \return The resulting vector. template -vec operator-(vec lhs, const vec& rhs) +vec operator-(vec lhs, const vec & rhs) { - lhs -= rhs; + lhs -= rhs; - return lhs; + return lhs; } #if defined(_MSC_VER) - #pragma warning(push) +#pragma warning(push) - // The operator* and operator/ throw a warning when a vec*f is multiplied by a double. - #pragma warning(disable:4244) +// The operator* and operator/ throw a warning when a vec*f is multiplied by a double. +#pragma warning(disable : 4244) #endif /// \brief Multiplies a vector by a scalar. Done both ways for python @@ -1238,11 +1082,11 @@ vec operator-(vec lhs, const vec& rhs) /// \param[in] rhs The vector. /// \return The result. template -vec operator*(vec lhs, const S& rhs) +vec operator*(vec lhs, const S & rhs) { - lhs *= rhs; + lhs *= rhs; - return lhs; + return lhs; } /// \brief Multiplies a vector by a scalar. Done both ways for Python @@ -1251,11 +1095,11 @@ vec operator*(vec lhs, const S& rhs) /// \param[in] rhs The scalar. /// \return The result. template -vec operator*(const S& rhs, vec lhs) +vec operator*(const S & rhs, vec lhs) { - lhs *= rhs; + lhs *= rhs; - return lhs; + return lhs; } /// \brief Divides a vector by a scalar. @@ -1264,15 +1108,15 @@ vec operator*(const S& rhs, vec lhs) /// \param[in] rhs The scalar. /// \return The result. template -vec operator/(vec lhs, const S& rhs) +vec operator/(vec lhs, const S & rhs) { - lhs /= rhs; + lhs /= rhs; - return lhs; + return lhs; } -#if defined (_MSC_VER) - #pragma warning(pop) +#if defined(_MSC_VER) +#pragma warning(pop) #endif // Specific Typedefs ////////////////////////////////////////////////////////// @@ -1341,20 +1185,19 @@ typedef vec<4, uint32_t> vec4u32; /// /// \param[in] v The vector to convert to string. /// \return The string representation. -template std::string str(vec v) +template +std::string str(vec v) { - std::stringstream ss; - ss << "("; - for (size_t i = 0; i < v.dim(); i++) - { - ss << v[i]; - - if (i + 1 < v.dim()) - ss << "; "; - } - ss << ")"; - - return ss.str(); + std::stringstream ss; + ss << "("; + for (size_t i = 0; i < v.dim(); i++) { + ss << v[i]; + + if (i + 1 < v.dim()) ss << "; "; + } + ss << ")"; + + return ss.str(); } /// \brief Overloads the ostream << operator for easy usage in displaying @@ -1363,13 +1206,14 @@ template std::string str(vec v) /// \param[in] out The ostream being output to. /// \param[in] v The vector to output to ostream. /// \return Reference to the current ostream. -template std::ostream& operator<<(std::ostream& out, vec v) +template +std::ostream & operator<<(std::ostream & out, vec v) { - out << str(v); - return out; + out << str(v); + return out; } -} -} +} // namespace math +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/vntime.h b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/vntime.h index 9f96b730..013ac4c9 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/include/vn/vntime.h +++ b/vectornav/vnproglib-1.2.0.0/cpp/include/vn/vntime.h @@ -1,60 +1,59 @@ #ifndef _VNXPLAT_TIME_H_ #define _VNXPLAT_TIME_H_ -#include "int.h" #include "export.h" +#include "int.h" -namespace vn { -namespace xplat { +namespace vn +{ +namespace xplat +{ struct vn_proglib_DLLEXPORT TimeStamp { public: - TimeStamp(); + TimeStamp(); private: - TimeStamp(int64_t sec, uint64_t usec); + TimeStamp(int64_t sec, uint64_t usec); public: - - // \brief Returns a timestamp. - // - // \return The timestamp. - static TimeStamp get(); - -// HACK: Current values are made public until the TimeStamp interface -// is fully worked out. -//private: + // \brief Returns a timestamp. + // + // \return The timestamp. + static TimeStamp get(); + + // HACK: Current values are made public until the TimeStamp interface + // is fully worked out. + //private: public: - int64_t _sec; // Seconds. - uint64_t _usec; // Microseconds. + int64_t _sec; // Seconds. + uint64_t _usec; // Microseconds. }; /// \brief Provides simple timing capabilities. class vn_proglib_DLLEXPORT Stopwatch { - public: - - /// \brief Creates a new Stopwatch and starts timing. - Stopwatch(); + /// \brief Creates a new Stopwatch and starts timing. + Stopwatch(); - ~Stopwatch(); + ~Stopwatch(); - /// \brief Resets the Stopwatch. - void reset(); + /// \brief Resets the Stopwatch. + void reset(); - /// \brief Gets the elapsed time in milliseconds. - /// - /// \return The elapsed time in milliseconds. - float elapsedMs(); + /// \brief Gets the elapsed time in milliseconds. + /// + /// \return The elapsed time in milliseconds. + float elapsedMs(); private: - struct Impl; - Impl *_pi; + struct Impl; + Impl * _pi; }; -} -} +} // namespace xplat +} // namespace vn #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/attitude.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/attitude.cpp index c9b692ba..7ec703cc 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/attitude.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/attitude.cpp @@ -1,119 +1,106 @@ #include "vn/attitude.h" -#include "vn/exceptions.h" + #include "vn/conversions.h" +#include "vn/exceptions.h" using namespace std; -namespace vn { -namespace math { +namespace vn +{ +namespace math +{ AttitudeF AttitudeF::noRotation() { - vec4f q(0, 0, 0, 1); + vec4f q(0, 0, 0, 1); - return AttitudeF(ATT_Quat, &q); + return AttitudeF(ATT_Quat, &q); } -AttitudeF AttitudeF::fromQuat(vec4f quat) -{ - return AttitudeF(ATT_Quat, &quat); -} +AttitudeF AttitudeF::fromQuat(vec4f quat) { return AttitudeF(ATT_Quat, &quat); } -AttitudeF AttitudeF::fromYprInDegs(vec3f yprInDegs) -{ - return AttitudeF(ATT_YprDegs, &yprInDegs); -} +AttitudeF AttitudeF::fromYprInDegs(vec3f yprInDegs) { return AttitudeF(ATT_YprDegs, &yprInDegs); } -AttitudeF AttitudeF::fromYprInRads(vec3f yprInRads) -{ - return AttitudeF(ATT_YprRads, &yprInRads); -} +AttitudeF AttitudeF::fromYprInRads(vec3f yprInRads) { return AttitudeF(ATT_YprRads, &yprInRads); } -AttitudeF AttitudeF::fromDcm(mat3f dcm) -{ - return AttitudeF(ATT_Dcm, &dcm); -} +AttitudeF AttitudeF::fromDcm(mat3f dcm) { return AttitudeF(ATT_Dcm, &dcm); } -AttitudeF::AttitudeF(AttitudeType type, void* attitude) : - _underlyingType(type) +AttitudeF::AttitudeF(AttitudeType type, void * attitude) : _underlyingType(type) { - size_t numOfBytesToCopy = sizeof(vec3f); + size_t numOfBytesToCopy = sizeof(vec3f); - if (_underlyingType == ATT_Quat) - numOfBytesToCopy = sizeof(vec4f); - else if (_underlyingType == ATT_Dcm) - numOfBytesToCopy = sizeof(mat3f); + if (_underlyingType == ATT_Quat) + numOfBytesToCopy = sizeof(vec4f); + else if (_underlyingType == ATT_Dcm) + numOfBytesToCopy = sizeof(mat3f); - copy(static_cast(attitude), static_cast(attitude) + numOfBytesToCopy, _data); + copy( + static_cast(attitude), static_cast(attitude) + numOfBytesToCopy, _data); } vec3f AttitudeF::yprInDegs() { - switch (_underlyingType) - { - case ATT_YprDegs: - return *static_cast(static_cast(_data)); - case ATT_YprRads: - return rad2deg(*static_cast(static_cast(_data))); - case ATT_Quat: - return quat2YprInDegs(*static_cast(static_cast(_data))); - case ATT_Dcm: - return dcm2YprInDegs(*static_cast(static_cast(_data))); - default: - throw not_implemented(); - } + switch (_underlyingType) { + case ATT_YprDegs: + return *static_cast(static_cast(_data)); + case ATT_YprRads: + return rad2deg(*static_cast(static_cast(_data))); + case ATT_Quat: + return quat2YprInDegs(*static_cast(static_cast(_data))); + case ATT_Dcm: + return dcm2YprInDegs(*static_cast(static_cast(_data))); + default: + throw not_implemented(); + } } vec3f AttitudeF::yprInRads() { - switch (_underlyingType) - { - case ATT_YprRads: - return *static_cast(static_cast(_data)); - case ATT_YprDegs: - return deg2rad(*static_cast(static_cast(_data))); - case ATT_Quat: - return quat2YprInRads(*static_cast(static_cast(_data))); - case ATT_Dcm: - return dcm2YprInRads(*static_cast(static_cast(_data))); - default: - throw not_implemented(); - } + switch (_underlyingType) { + case ATT_YprRads: + return *static_cast(static_cast(_data)); + case ATT_YprDegs: + return deg2rad(*static_cast(static_cast(_data))); + case ATT_Quat: + return quat2YprInRads(*static_cast(static_cast(_data))); + case ATT_Dcm: + return dcm2YprInRads(*static_cast(static_cast(_data))); + default: + throw not_implemented(); + } } vec4f AttitudeF::quat() { - switch (_underlyingType) - { - case ATT_Quat: - return *static_cast(static_cast(_data)); - case ATT_YprDegs: - return yprInDegs2Quat(*static_cast(static_cast(_data))); - case ATT_YprRads: - return yprInRads2Quat(*static_cast(static_cast(_data))); - case ATT_Dcm: - return dcm2quat(*static_cast(static_cast(_data))); - default: - throw not_implemented(); - } + switch (_underlyingType) { + case ATT_Quat: + return *static_cast(static_cast(_data)); + case ATT_YprDegs: + return yprInDegs2Quat(*static_cast(static_cast(_data))); + case ATT_YprRads: + return yprInRads2Quat(*static_cast(static_cast(_data))); + case ATT_Dcm: + return dcm2quat(*static_cast(static_cast(_data))); + default: + throw not_implemented(); + } } mat3f AttitudeF::dcm() { - switch (_underlyingType) - { - case ATT_Dcm: - return *static_cast(static_cast(_data)); - case ATT_YprDegs: - return yprInDegs2Dcm(*static_cast(static_cast(_data))); - case ATT_YprRads: - return yprInRads2Dcm(*static_cast(static_cast(_data))); - case ATT_Quat: - return quat2dcm(*static_cast(static_cast(_data))); - default: - throw not_implemented(); - } + switch (_underlyingType) { + case ATT_Dcm: + return *static_cast(static_cast(_data)); + case ATT_YprDegs: + return yprInDegs2Dcm(*static_cast(static_cast(_data))); + case ATT_YprRads: + return yprInRads2Dcm(*static_cast(static_cast(_data))); + case ATT_Quat: + return quat2dcm(*static_cast(static_cast(_data))); + default: + throw not_implemented(); + } } -} -} +} // namespace math +} // namespace vn diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/benchmark.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/benchmark.cpp index e7aabaa4..a3486792 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/benchmark.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/benchmark.cpp @@ -5,22 +5,22 @@ #endif #include + #include #include "hayai_main.hpp" -int main(int argc, char* argv[]) +int main(int argc, char * argv[]) { - // Set up the main runner. - ::hayai::MainRunner runner; + // Set up the main runner. + ::hayai::MainRunner runner; - // Parse the arguments. - int result = runner.ParseArgs(argc, argv); - if (result) - { - return result; - } + // Parse the arguments. + int result = runner.ParseArgs(argc, argv); + if (result) { + return result; + } - // Execute based on the selected mode. - return runner.Run(); + // Execute based on the selected mode. + return runner.Run(); } diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/compositedata.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/compositedata.cpp index 4a77ec45..5bc999d9 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/compositedata.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/compositedata.cpp @@ -1,146 +1,136 @@ #include "vn/compositedata.h" + #include "vn/conversions.h" using namespace std; using namespace vn::math; using namespace vn::protocol::uart; -namespace vn { -namespace sensors { +namespace vn +{ +namespace sensors +{ -typedef vector::iterator cditer; +typedef vector::iterator cditer; struct CompositeData::Impl { - enum AttitudeType - { - CDATT_None, - CDATT_YawPitchRoll, - CDATT_Quaternion, - CDATT_DirectionCosineMatrix - }; - - enum AccelerationType - { - CDACC_None, - CDACC_Normal, - CDACC_Uncompensated, - CDACC_LinearBody, - CDACC_LinearNed, - CDACC_Ned, - CDACC_Ecef, - CDACC_LinearEcef, - }; - - enum MagneticType - { - CDMAG_None, - CDMAG_Normal, - CDMAG_Uncompensated, - CDMAG_Ned, - CDMAG_Ecef, - }; - - enum AngularRateType - { - CDANR_None, - CDANR_Normal, - CDANR_Uncompensated, - }; - - enum TemperatureType - { - CDTEM_None, - CDTEM_Normal, - }; - - enum PressureType - { - CDPRE_None, - CDPRE_Normal - }; - - enum PositionType - { - CDPOS_None, + enum AttitudeType { + CDATT_None, + CDATT_YawPitchRoll, + CDATT_Quaternion, + CDATT_DirectionCosineMatrix + }; + + enum AccelerationType { + CDACC_None, + CDACC_Normal, + CDACC_Uncompensated, + CDACC_LinearBody, + CDACC_LinearNed, + CDACC_Ned, + CDACC_Ecef, + CDACC_LinearEcef, + }; + + enum MagneticType { + CDMAG_None, + CDMAG_Normal, + CDMAG_Uncompensated, + CDMAG_Ned, + CDMAG_Ecef, + }; + + enum AngularRateType { + CDANR_None, + CDANR_Normal, + CDANR_Uncompensated, + }; + + enum TemperatureType { + CDTEM_None, + CDTEM_Normal, + }; + + enum PressureType { CDPRE_None, CDPRE_Normal }; + + enum PositionType { + CDPOS_None, CDPOS_GpsLla, CDPOS_Gps2Lla, CDPOS_GpsEcef, CDPOS_Gps2Ecef, CDPOS_EstimatedLla, - CDPOS_EstimatedEcef - }; + CDPOS_EstimatedEcef + }; - enum VelocityType - { - CDVEL_None, + enum VelocityType { + CDVEL_None, CDVEL_GpsNed, CDVEL_Gps2Ned, CDVEL_GpsEcef, CDVEL_Gps2Ecef, CDVEL_EstimatedNed, - CDVEL_EstimatedEcef, - CDVEL_EstimatedBody - }; + CDVEL_EstimatedEcef, + CDVEL_EstimatedBody + }; - enum PositionUncertaintyType - { - CDPOU_None, + enum PositionUncertaintyType { + CDPOU_None, CDPOU_GpsNed, CDPOU_Gps2Ned, CDPOU_GpsEcef, CDPOU_Gps2Ecef, CDPOU_Estimated - }; - - enum VelocityUncertaintyType - { - CDVEU_None, - CDVEU_Gps, - CDVEU_Gps2, - CDVEU_Estimated - }; - - AttitudeType mostRecentlyUpdatedAttitudeType; - MagneticType mostRecentlyUpdatedMagneticType; - AccelerationType mostRecentlyUpdatedAccelerationType; - AngularRateType mostRecentlyUpdatedAngularRateType; - TemperatureType mostRecentlyUpdatedTemperatureType; - PressureType mostRecentlyUpdatePressureType; - PositionType mostRecentlyUpdatedPositionType; - VelocityType mostRecentlyUpdatedVelocityType; - PositionUncertaintyType mostRecentlyUpdatedPositionUncertaintyType; - VelocityUncertaintyType mostRecentlyUpdatedVelocityUncertaintyType; - bool hasYawPitchRoll, hasQuaternion, hasDirectionCosineMatrix, - hasMagnetic, hasMagneticUncompensated, hasMagneticNed, hasMagneticEcef, - hasAcceleration, hasAccelerationLinearBody, hasAccelerationUncompensated, hasAccelerationLinearNed, hasAccelerationLinearEcef, hasAccelerationNed, hasAccelerationEcef, - hasAngularRate, hasAngularRateUncompensated, - hasTemperature, hasPressure, - hasPositionGpsLla, hasPositionGps2Lla, hasPositionGpsEcef, hasPositionGps2Ecef, hasPositionEstimatedLla, hasPositionEstimatedEcef, - hasVelocityGpsNed, hasVelocityGps2Ned, hasVelocityGpsEcef, hasVelocityGps2Ecef, hasVelocityEstimatedNed, hasVelocityEstimatedEcef, hasVelocityEstimatedBody, - hasDeltaTime, hasDeltaTheta, hasDeltaVelocity, - hasTimeStartup, hasTimeGps, hasTimeGps2, hasTow, hasWeek, hasGpsWeek, hasGps2Week, hasNumSats, hasNumSats2, hasTimeSyncIn, hasVpeStatus, hasInsStatus, - hasSyncInCnt, hasSyncOutCnt, hasTimeStatus, hasTimeGpsPps, hasTimeGps2Pps, hasGpsTow, hasGps2Tow, hasTimeUtc, hasTimeUtc2, hasSensSat, hasFix, hasFix2, - hasPositionUncertaintyGpsNed, hasPositionUncertaintyGps2Ned, hasPositionUncertaintyGpsEcef, hasPositionUncertaintyGps2Ecef, hasPositionUncertaintyEstimated, - hasVelocityUncertaintyGps, hasVelocityUncertaintyGps2, hasVelocityUncertaintyEstimated, hasTimeUncertainty, hasTimeUncertainty2, hasAttitudeUncertainty, - hasTimeInfo, hasTimeInfo2, hasDop, hasDop2; - vec3f yawPitchRoll, - magnetic, magneticUncompensated, magneticNed, magneticEcef, - acceleration, accelerationLinearBody, accelerationUncompensated, accelerationLinearNed, accelerationLinearEcef, accelerationNed, accelerationEcef, - angularRate, angularRateUncompensated, - velocityGpsNed, velocityGps2Ned, velocityGpsEcef, velocityGps2Ecef, velocityEstimatedNed, velocityEstimatedEcef, velocityEstimatedBody, - deltaTheta, deltaVelocity, positionUncertaintyGpsNed, positionUncertaintyGps2Ned, positionUncertaintyGpsEcef, positionUncertaintyGps2Ecef, attitudeUncertainty; - vec3d positionGpsLla, positionGps2Lla, positionGpsEcef, positionGps2Ecef, positionEstimatedLla, positionEstimatedEcef; - vec4f quaternion; - mat3f directionConsineMatrix; - float temperature, pressure, deltaTime, positionUncertaintyEstimated, - velocityUncertaintyGps, velocityUncertaintyGps2, velocityUncertaintyEstimated; - uint64_t timeStartup, timeGps, timeGps2, timeSyncIn, timeGpsPps, timeGps2Pps, gpsTow, gps2Tow; - double tow; - uint16_t week, gpsWeek, gps2Week; - uint8_t numSats, numSats2, timeStatus; - VpeStatus vpeStatus; - InsStatus insStatus; + }; + + enum VelocityUncertaintyType { CDVEU_None, CDVEU_Gps, CDVEU_Gps2, CDVEU_Estimated }; + + AttitudeType mostRecentlyUpdatedAttitudeType; + MagneticType mostRecentlyUpdatedMagneticType; + AccelerationType mostRecentlyUpdatedAccelerationType; + AngularRateType mostRecentlyUpdatedAngularRateType; + TemperatureType mostRecentlyUpdatedTemperatureType; + PressureType mostRecentlyUpdatePressureType; + PositionType mostRecentlyUpdatedPositionType; + VelocityType mostRecentlyUpdatedVelocityType; + PositionUncertaintyType mostRecentlyUpdatedPositionUncertaintyType; + VelocityUncertaintyType mostRecentlyUpdatedVelocityUncertaintyType; + bool hasYawPitchRoll, hasQuaternion, hasDirectionCosineMatrix, hasMagnetic, + hasMagneticUncompensated, hasMagneticNed, hasMagneticEcef, hasAcceleration, + hasAccelerationLinearBody, hasAccelerationUncompensated, hasAccelerationLinearNed, + hasAccelerationLinearEcef, hasAccelerationNed, hasAccelerationEcef, hasAngularRate, + hasAngularRateUncompensated, hasTemperature, hasPressure, hasPositionGpsLla, hasPositionGps2Lla, + hasPositionGpsEcef, hasPositionGps2Ecef, hasPositionEstimatedLla, hasPositionEstimatedEcef, + hasVelocityGpsNed, hasVelocityGps2Ned, hasVelocityGpsEcef, hasVelocityGps2Ecef, + hasVelocityEstimatedNed, hasVelocityEstimatedEcef, hasVelocityEstimatedBody, hasDeltaTime, + hasDeltaTheta, hasDeltaVelocity, hasTimeStartup, hasTimeGps, hasTimeGps2, hasTow, hasWeek, + hasGpsWeek, hasGps2Week, hasNumSats, hasNumSats2, hasTimeSyncIn, hasVpeStatus, hasInsStatus, + hasSyncInCnt, hasSyncOutCnt, hasTimeStatus, hasTimeGpsPps, hasTimeGps2Pps, hasGpsTow, + hasGps2Tow, hasTimeUtc, hasTimeUtc2, hasSensSat, hasFix, hasFix2, hasPositionUncertaintyGpsNed, + hasPositionUncertaintyGps2Ned, hasPositionUncertaintyGpsEcef, hasPositionUncertaintyGps2Ecef, + hasPositionUncertaintyEstimated, hasVelocityUncertaintyGps, hasVelocityUncertaintyGps2, + hasVelocityUncertaintyEstimated, hasTimeUncertainty, hasTimeUncertainty2, + hasAttitudeUncertainty, hasTimeInfo, hasTimeInfo2, hasDop, hasDop2; + vec3f yawPitchRoll, magnetic, magneticUncompensated, magneticNed, magneticEcef, acceleration, + accelerationLinearBody, accelerationUncompensated, accelerationLinearNed, + accelerationLinearEcef, accelerationNed, accelerationEcef, angularRate, + angularRateUncompensated, velocityGpsNed, velocityGps2Ned, velocityGpsEcef, velocityGps2Ecef, + velocityEstimatedNed, velocityEstimatedEcef, velocityEstimatedBody, deltaTheta, deltaVelocity, + positionUncertaintyGpsNed, positionUncertaintyGps2Ned, positionUncertaintyGpsEcef, + positionUncertaintyGps2Ecef, attitudeUncertainty; + vec3d positionGpsLla, positionGps2Lla, positionGpsEcef, positionGps2Ecef, positionEstimatedLla, + positionEstimatedEcef; + vec4f quaternion; + mat3f directionConsineMatrix; + float temperature, pressure, deltaTime, positionUncertaintyEstimated, velocityUncertaintyGps, + velocityUncertaintyGps2, velocityUncertaintyEstimated; + uint64_t timeStartup, timeGps, timeGps2, timeSyncIn, timeGpsPps, timeGps2Pps, gpsTow, gps2Tow; + double tow; + uint16_t week, gpsWeek, gps2Week; + uint8_t numSats, numSats2, timeStatus; + VpeStatus vpeStatus; + InsStatus insStatus; uint32_t syncInCnt, syncOutCnt, timeUncertainty, timeUncertainty2; GpsFix fix; GpsFix fix2; @@ -152,31 +142,31 @@ struct CompositeData::Impl GnssDop dop; GnssDop dop2; - Impl& operator=(Impl& aImpl) - { - yawPitchRoll = aImpl.yawPitchRoll; - magnetic = aImpl.magnetic; - magneticUncompensated = aImpl.magneticUncompensated; - magneticNed = aImpl.magneticNed; - magneticEcef = aImpl.magneticEcef; - acceleration = aImpl.acceleration; - accelerationLinearBody = aImpl.accelerationLinearBody; - accelerationUncompensated = aImpl.accelerationUncompensated; - accelerationLinearNed = aImpl.accelerationLinearNed; - accelerationLinearEcef = aImpl.accelerationLinearEcef; - accelerationNed = aImpl.accelerationNed; - accelerationEcef = aImpl.accelerationEcef; - angularRate = aImpl.angularRate; - angularRateUncompensated = aImpl.angularRateUncompensated; + Impl & operator=(Impl & aImpl) + { + yawPitchRoll = aImpl.yawPitchRoll; + magnetic = aImpl.magnetic; + magneticUncompensated = aImpl.magneticUncompensated; + magneticNed = aImpl.magneticNed; + magneticEcef = aImpl.magneticEcef; + acceleration = aImpl.acceleration; + accelerationLinearBody = aImpl.accelerationLinearBody; + accelerationUncompensated = aImpl.accelerationUncompensated; + accelerationLinearNed = aImpl.accelerationLinearNed; + accelerationLinearEcef = aImpl.accelerationLinearEcef; + accelerationNed = aImpl.accelerationNed; + accelerationEcef = aImpl.accelerationEcef; + angularRate = aImpl.angularRate; + angularRateUncompensated = aImpl.angularRateUncompensated; velocityGpsNed = aImpl.velocityGpsNed; velocityGps2Ned = aImpl.velocityGps2Ned; velocityGpsEcef = aImpl.velocityGpsEcef; velocityGps2Ecef = aImpl.velocityGps2Ecef; velocityEstimatedNed = aImpl.velocityEstimatedNed; - velocityEstimatedEcef = aImpl.velocityEstimatedEcef; - velocityEstimatedBody = aImpl.velocityEstimatedBody; - deltaTheta = aImpl.deltaTheta; - deltaVelocity = aImpl.deltaVelocity; + velocityEstimatedEcef = aImpl.velocityEstimatedEcef; + velocityEstimatedBody = aImpl.velocityEstimatedBody; + deltaTheta = aImpl.deltaTheta; + deltaVelocity = aImpl.deltaVelocity; positionUncertaintyGpsNed = aImpl.positionUncertaintyGpsNed; positionUncertaintyGps2Ned = aImpl.positionUncertaintyGps2Ned; positionUncertaintyGpsEcef = aImpl.positionUncertaintyGpsEcef; @@ -187,18 +177,18 @@ struct CompositeData::Impl positionGpsEcef = aImpl.positionGpsEcef; positionGps2Ecef = aImpl.positionGps2Ecef; positionEstimatedLla = aImpl.positionEstimatedLla; - positionEstimatedEcef = aImpl.positionEstimatedEcef; - quaternion = aImpl.quaternion; - directionConsineMatrix = aImpl.directionConsineMatrix; - - temperature = aImpl.temperature; - pressure = aImpl.pressure; - deltaTime = aImpl.deltaTime; - positionUncertaintyEstimated = aImpl.positionUncertaintyEstimated; + positionEstimatedEcef = aImpl.positionEstimatedEcef; + quaternion = aImpl.quaternion; + directionConsineMatrix = aImpl.directionConsineMatrix; + + temperature = aImpl.temperature; + pressure = aImpl.pressure; + deltaTime = aImpl.deltaTime; + positionUncertaintyEstimated = aImpl.positionUncertaintyEstimated; velocityUncertaintyGps = aImpl.velocityUncertaintyGps; velocityUncertaintyGps2 = aImpl.velocityUncertaintyGps2; velocityUncertaintyEstimated = aImpl.velocityUncertaintyEstimated; - timeStartup = aImpl.timeStartup; + timeStartup = aImpl.timeStartup; timeGps = aImpl.timeGps; timeGps2 = aImpl.timeGps2; timeUtc = aImpl.timeUtc; @@ -215,62 +205,62 @@ struct CompositeData::Impl numSats = aImpl.numSats; numSats2 = aImpl.numSats2; insStatus = aImpl.insStatus; - syncInCnt = aImpl.syncInCnt; + syncInCnt = aImpl.syncInCnt; syncOutCnt = aImpl.syncOutCnt; timeStatus = aImpl.timeStatus; timeUncertainty = aImpl.timeUncertainty; timeUncertainty2 = aImpl.timeUncertainty2; fix = aImpl.fix; - sensSat = aImpl.sensSat; + sensSat = aImpl.sensSat; timeInfo = aImpl.timeInfo; dop = aImpl.dop; dop2 = aImpl.dop2; - mostRecentlyUpdatedAttitudeType = aImpl.mostRecentlyUpdatedAttitudeType; - mostRecentlyUpdatedMagneticType = aImpl.mostRecentlyUpdatedMagneticType; - mostRecentlyUpdatedAccelerationType = aImpl.mostRecentlyUpdatedAccelerationType; - mostRecentlyUpdatedAngularRateType = aImpl.mostRecentlyUpdatedAngularRateType; - mostRecentlyUpdatedTemperatureType = aImpl.mostRecentlyUpdatedTemperatureType; - mostRecentlyUpdatePressureType = aImpl.mostRecentlyUpdatePressureType; - mostRecentlyUpdatedPositionType = aImpl.mostRecentlyUpdatedPositionType; - mostRecentlyUpdatedVelocityType = aImpl.mostRecentlyUpdatedVelocityType; - mostRecentlyUpdatedPositionUncertaintyType = aImpl.mostRecentlyUpdatedPositionUncertaintyType; - mostRecentlyUpdatedVelocityUncertaintyType = aImpl.mostRecentlyUpdatedVelocityUncertaintyType; - hasYawPitchRoll = aImpl.hasYawPitchRoll; - hasQuaternion = aImpl.hasQuaternion; - hasDirectionCosineMatrix = aImpl.hasDirectionCosineMatrix; - hasMagnetic = aImpl.hasMagnetic; - hasMagneticUncompensated = aImpl.hasMagneticUncompensated; - hasMagneticNed = aImpl.hasMagneticNed; - hasMagneticEcef = aImpl.hasMagneticEcef; - hasAcceleration = aImpl.hasAcceleration; - hasAccelerationLinearBody = aImpl.hasAccelerationLinearBody; - hasAccelerationUncompensated = aImpl.hasAccelerationUncompensated; - hasAccelerationLinearNed = aImpl.hasAccelerationLinearNed; - hasAccelerationLinearEcef = aImpl.hasAccelerationLinearEcef; - hasAccelerationNed = aImpl.hasAccelerationNed; - hasAccelerationEcef = aImpl.hasAccelerationEcef; - hasAngularRate = aImpl.hasAngularRate; - hasAngularRateUncompensated = aImpl.hasAngularRateUncompensated; - hasTemperature = aImpl.hasTemperature; - hasPressure = aImpl.hasPressure; + mostRecentlyUpdatedAttitudeType = aImpl.mostRecentlyUpdatedAttitudeType; + mostRecentlyUpdatedMagneticType = aImpl.mostRecentlyUpdatedMagneticType; + mostRecentlyUpdatedAccelerationType = aImpl.mostRecentlyUpdatedAccelerationType; + mostRecentlyUpdatedAngularRateType = aImpl.mostRecentlyUpdatedAngularRateType; + mostRecentlyUpdatedTemperatureType = aImpl.mostRecentlyUpdatedTemperatureType; + mostRecentlyUpdatePressureType = aImpl.mostRecentlyUpdatePressureType; + mostRecentlyUpdatedPositionType = aImpl.mostRecentlyUpdatedPositionType; + mostRecentlyUpdatedVelocityType = aImpl.mostRecentlyUpdatedVelocityType; + mostRecentlyUpdatedPositionUncertaintyType = aImpl.mostRecentlyUpdatedPositionUncertaintyType; + mostRecentlyUpdatedVelocityUncertaintyType = aImpl.mostRecentlyUpdatedVelocityUncertaintyType; + hasYawPitchRoll = aImpl.hasYawPitchRoll; + hasQuaternion = aImpl.hasQuaternion; + hasDirectionCosineMatrix = aImpl.hasDirectionCosineMatrix; + hasMagnetic = aImpl.hasMagnetic; + hasMagneticUncompensated = aImpl.hasMagneticUncompensated; + hasMagneticNed = aImpl.hasMagneticNed; + hasMagneticEcef = aImpl.hasMagneticEcef; + hasAcceleration = aImpl.hasAcceleration; + hasAccelerationLinearBody = aImpl.hasAccelerationLinearBody; + hasAccelerationUncompensated = aImpl.hasAccelerationUncompensated; + hasAccelerationLinearNed = aImpl.hasAccelerationLinearNed; + hasAccelerationLinearEcef = aImpl.hasAccelerationLinearEcef; + hasAccelerationNed = aImpl.hasAccelerationNed; + hasAccelerationEcef = aImpl.hasAccelerationEcef; + hasAngularRate = aImpl.hasAngularRate; + hasAngularRateUncompensated = aImpl.hasAngularRateUncompensated; + hasTemperature = aImpl.hasTemperature; + hasPressure = aImpl.hasPressure; hasPositionGpsLla = aImpl.hasPositionGpsLla; hasPositionGps2Lla = aImpl.hasPositionGps2Lla; hasPositionGpsEcef = aImpl.hasPositionGpsEcef; hasPositionGps2Ecef = aImpl.hasPositionGps2Ecef; hasPositionEstimatedLla = aImpl.hasPositionEstimatedLla; - hasPositionEstimatedEcef = aImpl.hasPositionEstimatedEcef; + hasPositionEstimatedEcef = aImpl.hasPositionEstimatedEcef; hasVelocityGpsNed = aImpl.hasVelocityGpsNed; hasVelocityGps2Ned = aImpl.hasVelocityGps2Ned; hasVelocityGpsEcef = aImpl.hasVelocityGpsEcef; hasVelocityGps2Ecef = aImpl.hasVelocityGps2Ecef; hasVelocityEstimatedNed = aImpl.hasVelocityEstimatedNed; - hasVelocityEstimatedEcef = aImpl.hasVelocityEstimatedEcef; - hasVelocityEstimatedBody = aImpl.hasVelocityEstimatedBody; - hasDeltaTime = aImpl.hasDeltaTime; - hasDeltaTheta = aImpl.hasDeltaTheta; - hasDeltaVelocity = aImpl.hasDeltaVelocity; - hasTimeStartup = aImpl.hasTimeStartup; + hasVelocityEstimatedEcef = aImpl.hasVelocityEstimatedEcef; + hasVelocityEstimatedBody = aImpl.hasVelocityEstimatedBody; + hasDeltaTime = aImpl.hasDeltaTime; + hasDeltaTheta = aImpl.hasDeltaTheta; + hasDeltaVelocity = aImpl.hasDeltaVelocity; + hasTimeStartup = aImpl.hasTimeStartup; hasTimeGps = aImpl.hasTimeGps; hasTimeGps2 = aImpl.hasTimeGps2; hasTow = aImpl.hasTow; @@ -280,9 +270,9 @@ struct CompositeData::Impl hasNumSats = aImpl.hasNumSats; hasNumSats2 = aImpl.hasNumSats2; hasTimeSyncIn = aImpl.hasTimeSyncIn; - hasVpeStatus = aImpl.hasVpeStatus; - hasInsStatus = aImpl.hasInsStatus; - hasSyncInCnt = aImpl.hasSyncInCnt; + hasVpeStatus = aImpl.hasVpeStatus; + hasInsStatus = aImpl.hasInsStatus; + hasSyncInCnt = aImpl.hasSyncInCnt; hasSyncOutCnt = aImpl.hasSyncOutCnt; hasTimeStatus = aImpl.hasTimeStatus; hasTimeGpsPps = aImpl.hasTimeGpsPps; @@ -292,7 +282,7 @@ struct CompositeData::Impl hasTimeUtc = aImpl.hasTimeUtc; hasTimeUtc2 = aImpl.hasTimeUtc2; hasSensSat = aImpl.hasSensSat; - hasFix = aImpl.hasFix; + hasFix = aImpl.hasFix; hasPositionUncertaintyGpsNed = aImpl.hasPositionUncertaintyGpsNed; hasPositionUncertaintyGps2Ned = aImpl.hasPositionUncertaintyGps2Ned; hasPositionUncertaintyGpsEcef = aImpl.hasPositionUncertaintyGpsEcef; @@ -309,56 +299,56 @@ struct CompositeData::Impl hasDop = aImpl.hasDop; hasDop2 = aImpl.hasDop2; - return *this; - } - - void reset() - { - mostRecentlyUpdatedAttitudeType = CDATT_None; - mostRecentlyUpdatedMagneticType = CDMAG_None; - mostRecentlyUpdatedAccelerationType = CDACC_None; - mostRecentlyUpdatedAngularRateType = CDANR_None; - mostRecentlyUpdatedTemperatureType = CDTEM_None; - mostRecentlyUpdatePressureType = CDPRE_None; - mostRecentlyUpdatedPositionType = CDPOS_None; - mostRecentlyUpdatedVelocityType = CDVEL_None; - mostRecentlyUpdatedPositionUncertaintyType = CDPOU_None; - mostRecentlyUpdatedVelocityUncertaintyType = CDVEU_None; - hasYawPitchRoll = false; - hasQuaternion = false; - hasDirectionCosineMatrix = false; - hasMagnetic = false; - hasMagneticUncompensated = false; - hasMagneticNed = false; - hasMagneticEcef = false; - hasAcceleration = false; - hasAccelerationLinearBody = false; - hasAccelerationUncompensated = false; - hasAccelerationLinearNed = false; - hasAccelerationLinearEcef = false; - hasAccelerationNed = false; - hasAccelerationEcef = false; - hasAngularRate = false; - hasAngularRateUncompensated = false; - hasTemperature = false; - hasPressure = false; + return *this; + } + + void reset() + { + mostRecentlyUpdatedAttitudeType = CDATT_None; + mostRecentlyUpdatedMagneticType = CDMAG_None; + mostRecentlyUpdatedAccelerationType = CDACC_None; + mostRecentlyUpdatedAngularRateType = CDANR_None; + mostRecentlyUpdatedTemperatureType = CDTEM_None; + mostRecentlyUpdatePressureType = CDPRE_None; + mostRecentlyUpdatedPositionType = CDPOS_None; + mostRecentlyUpdatedVelocityType = CDVEL_None; + mostRecentlyUpdatedPositionUncertaintyType = CDPOU_None; + mostRecentlyUpdatedVelocityUncertaintyType = CDVEU_None; + hasYawPitchRoll = false; + hasQuaternion = false; + hasDirectionCosineMatrix = false; + hasMagnetic = false; + hasMagneticUncompensated = false; + hasMagneticNed = false; + hasMagneticEcef = false; + hasAcceleration = false; + hasAccelerationLinearBody = false; + hasAccelerationUncompensated = false; + hasAccelerationLinearNed = false; + hasAccelerationLinearEcef = false; + hasAccelerationNed = false; + hasAccelerationEcef = false; + hasAngularRate = false; + hasAngularRateUncompensated = false; + hasTemperature = false; + hasPressure = false; hasPositionGpsLla = false; hasPositionGps2Lla = false; hasPositionGpsEcef = false; hasPositionGps2Ecef = false; hasPositionEstimatedLla = false; - hasPositionEstimatedEcef = false; + hasPositionEstimatedEcef = false; hasVelocityGpsNed = false; hasVelocityGps2Ned = false; hasVelocityGpsEcef = false; hasVelocityGps2Ecef = false; hasVelocityEstimatedNed = false; - hasVelocityEstimatedEcef = false; - hasVelocityEstimatedBody = false; - hasDeltaTime = false; - hasDeltaTheta = false; - hasDeltaVelocity = false; - hasTimeStartup = false; + hasVelocityEstimatedEcef = false; + hasVelocityEstimatedBody = false; + hasDeltaTime = false; + hasDeltaTheta = false; + hasDeltaVelocity = false; + hasTimeStartup = false; hasTimeGps = false; hasTimeGps2 = false; hasTow = false; @@ -368,9 +358,9 @@ struct CompositeData::Impl hasNumSats = false; hasNumSats2 = false; hasTimeSyncIn = false; - hasVpeStatus = false; - hasInsStatus = false; - hasSyncInCnt = false; + hasVpeStatus = false; + hasInsStatus = false; + hasSyncInCnt = false; hasTimeGpsPps = false; hasTimeGps2Pps = false; hasGpsTow = false; @@ -378,7 +368,7 @@ struct CompositeData::Impl hasTimeUtc = false; hasTimeUtc2 = false; hasSensSat = false; - hasFix = false; + hasFix = false; hasPositionUncertaintyGpsNed = false; hasPositionUncertaintyGps2Ned = false; hasPositionUncertaintyGpsEcef = false; @@ -396,135 +386,131 @@ struct CompositeData::Impl hasDop2 = false; } - void setYawPitchRoll(vec3f ypr) - { - mostRecentlyUpdatedAttitudeType = CDATT_YawPitchRoll; - hasYawPitchRoll = true; - yawPitchRoll = ypr; - } - - void setQuaternion(vec4f quat) - { - mostRecentlyUpdatedAttitudeType = CDATT_Quaternion; - hasQuaternion = true; - quaternion = quat; - } - - void setDirectionConsineMatrix(mat3f dcm) - { - mostRecentlyUpdatedAttitudeType = CDATT_DirectionCosineMatrix; - hasDirectionCosineMatrix = true; - directionConsineMatrix = dcm; - } - - void setMagnetic(vec3f mag) - { - mostRecentlyUpdatedMagneticType = CDMAG_Normal; - hasMagnetic = true; - magnetic = mag; - } - - void setMagneticUncompensated(vec3f mag) - { - mostRecentlyUpdatedMagneticType = CDMAG_Uncompensated; - hasMagneticUncompensated = true; - magneticUncompensated = mag; - } - - void setMagneticNed(vec3f mag) - { - mostRecentlyUpdatedMagneticType = CDMAG_Ned; - hasMagneticNed = true; - magneticNed = mag; - } - - void setMagneticEcef(vec3f mag) - { - mostRecentlyUpdatedMagneticType = CDMAG_Ecef; - hasMagneticEcef = true; - magneticEcef = mag; - } - - - void setAcceleration(vec3f accel) - { - mostRecentlyUpdatedAccelerationType = CDACC_Normal; - hasAcceleration = true; - acceleration = accel; - } - - void setAccelerationLinearBody(vec3f accel) - { - mostRecentlyUpdatedAccelerationType = CDACC_LinearBody; - hasAccelerationLinearBody = true; - accelerationLinearBody = accel; - } - - void setAccelerationUncompensated(vec3f accel) - { - mostRecentlyUpdatedAccelerationType = CDACC_Uncompensated; - hasAccelerationUncompensated = true; - accelerationUncompensated = accel; - } - - void setAccelerationLinearNed(vec3f accel) - { - mostRecentlyUpdatedAccelerationType = CDACC_LinearNed; - hasAccelerationLinearNed = true; - accelerationLinearNed = accel; - } - - void setAccelerationLinearEcef(vec3f accel) - { - mostRecentlyUpdatedAccelerationType = CDACC_LinearEcef; - hasAccelerationLinearEcef = true; - accelerationLinearEcef = accel; - } - - void setAccelerationNed(vec3f accel) - { - mostRecentlyUpdatedAccelerationType = CDACC_Ned; - hasAccelerationNed = true; - accelerationNed = accel; - } - - void setAccelerationEcef(vec3f accel) - { - mostRecentlyUpdatedAccelerationType = CDACC_Ecef; - hasAccelerationEcef = true; - accelerationEcef = accel; - } - - - void setAngularRate(vec3f ar) - { - mostRecentlyUpdatedAngularRateType = CDANR_Normal; - hasAngularRate = true; - angularRate = ar; - } - - void setAngularRateUncompensated(vec3f ar) - { - mostRecentlyUpdatedAngularRateType = CDANR_Uncompensated; - hasAngularRateUncompensated = true; - angularRateUncompensated = ar; - } - - - void setTemperature(float temp) - { - mostRecentlyUpdatedTemperatureType = CDTEM_Normal; - hasTemperature = true; - temperature = temp; - } - - - void setPressure(float pres) - { - mostRecentlyUpdatePressureType = CDPRE_Normal; - hasPressure = true; - pressure = pres; - } + void setYawPitchRoll(vec3f ypr) + { + mostRecentlyUpdatedAttitudeType = CDATT_YawPitchRoll; + hasYawPitchRoll = true; + yawPitchRoll = ypr; + } + + void setQuaternion(vec4f quat) + { + mostRecentlyUpdatedAttitudeType = CDATT_Quaternion; + hasQuaternion = true; + quaternion = quat; + } + + void setDirectionConsineMatrix(mat3f dcm) + { + mostRecentlyUpdatedAttitudeType = CDATT_DirectionCosineMatrix; + hasDirectionCosineMatrix = true; + directionConsineMatrix = dcm; + } + + void setMagnetic(vec3f mag) + { + mostRecentlyUpdatedMagneticType = CDMAG_Normal; + hasMagnetic = true; + magnetic = mag; + } + + void setMagneticUncompensated(vec3f mag) + { + mostRecentlyUpdatedMagneticType = CDMAG_Uncompensated; + hasMagneticUncompensated = true; + magneticUncompensated = mag; + } + + void setMagneticNed(vec3f mag) + { + mostRecentlyUpdatedMagneticType = CDMAG_Ned; + hasMagneticNed = true; + magneticNed = mag; + } + + void setMagneticEcef(vec3f mag) + { + mostRecentlyUpdatedMagneticType = CDMAG_Ecef; + hasMagneticEcef = true; + magneticEcef = mag; + } + + void setAcceleration(vec3f accel) + { + mostRecentlyUpdatedAccelerationType = CDACC_Normal; + hasAcceleration = true; + acceleration = accel; + } + + void setAccelerationLinearBody(vec3f accel) + { + mostRecentlyUpdatedAccelerationType = CDACC_LinearBody; + hasAccelerationLinearBody = true; + accelerationLinearBody = accel; + } + + void setAccelerationUncompensated(vec3f accel) + { + mostRecentlyUpdatedAccelerationType = CDACC_Uncompensated; + hasAccelerationUncompensated = true; + accelerationUncompensated = accel; + } + + void setAccelerationLinearNed(vec3f accel) + { + mostRecentlyUpdatedAccelerationType = CDACC_LinearNed; + hasAccelerationLinearNed = true; + accelerationLinearNed = accel; + } + + void setAccelerationLinearEcef(vec3f accel) + { + mostRecentlyUpdatedAccelerationType = CDACC_LinearEcef; + hasAccelerationLinearEcef = true; + accelerationLinearEcef = accel; + } + + void setAccelerationNed(vec3f accel) + { + mostRecentlyUpdatedAccelerationType = CDACC_Ned; + hasAccelerationNed = true; + accelerationNed = accel; + } + + void setAccelerationEcef(vec3f accel) + { + mostRecentlyUpdatedAccelerationType = CDACC_Ecef; + hasAccelerationEcef = true; + accelerationEcef = accel; + } + + void setAngularRate(vec3f ar) + { + mostRecentlyUpdatedAngularRateType = CDANR_Normal; + hasAngularRate = true; + angularRate = ar; + } + + void setAngularRateUncompensated(vec3f ar) + { + mostRecentlyUpdatedAngularRateType = CDANR_Uncompensated; + hasAngularRateUncompensated = true; + angularRateUncompensated = ar; + } + + void setTemperature(float temp) + { + mostRecentlyUpdatedTemperatureType = CDTEM_Normal; + hasTemperature = true; + temperature = temp; + } + + void setPressure(float pres) + { + mostRecentlyUpdatePressureType = CDPRE_Normal; + hasPressure = true; + pressure = pres; + } void setPositionGpsLla(vec3d pos) { @@ -555,18 +541,18 @@ struct CompositeData::Impl } void setPositionEstimatedLla(vec3d pos) - { - mostRecentlyUpdatedPositionType = CDPOS_EstimatedLla; - hasPositionEstimatedLla = true; - positionEstimatedLla = pos; - } - - void setPositionEstimatedEcef(vec3d pos) - { - mostRecentlyUpdatedPositionType = CDPOS_EstimatedEcef; - hasPositionEstimatedEcef = true; - positionEstimatedEcef = pos; - } + { + mostRecentlyUpdatedPositionType = CDPOS_EstimatedLla; + hasPositionEstimatedLla = true; + positionEstimatedLla = pos; + } + + void setPositionEstimatedEcef(vec3d pos) + { + mostRecentlyUpdatedPositionType = CDPOS_EstimatedEcef; + hasPositionEstimatedEcef = true; + positionEstimatedEcef = pos; + } void setVelocityGpsNed(vec3f vel) { @@ -597,49 +583,49 @@ struct CompositeData::Impl } void setVelocityEstimatedNed(vec3f vel) - { - mostRecentlyUpdatedVelocityType = CDVEL_EstimatedNed; - hasVelocityEstimatedNed = true; - velocityEstimatedNed = vel; - } - - void setVelocityEstimatedEcef(vec3f vel) - { - mostRecentlyUpdatedVelocityType = CDVEL_EstimatedEcef; - hasVelocityEstimatedEcef = true; - velocityEstimatedEcef = vel; - } - - void setVelocityEstimatedBody(vec3f vel) - { - mostRecentlyUpdatedVelocityType = CDVEL_EstimatedBody; - hasVelocityEstimatedBody = true; - velocityEstimatedBody = vel; - } - - void setDeltaTime(float time) - { - hasDeltaTime = true; - deltaTime = time; - } - - void setDeltaTheta(vec3f theta) - { - hasDeltaTheta = true; - deltaTheta = theta; - } - - void setDeltaVelocity(vec3f vel) - { - hasDeltaVelocity = true; - deltaVelocity = vel; - } - - void setTimeStartup(uint64_t ts) - { - hasTimeStartup = true; - timeStartup = ts; - } + { + mostRecentlyUpdatedVelocityType = CDVEL_EstimatedNed; + hasVelocityEstimatedNed = true; + velocityEstimatedNed = vel; + } + + void setVelocityEstimatedEcef(vec3f vel) + { + mostRecentlyUpdatedVelocityType = CDVEL_EstimatedEcef; + hasVelocityEstimatedEcef = true; + velocityEstimatedEcef = vel; + } + + void setVelocityEstimatedBody(vec3f vel) + { + mostRecentlyUpdatedVelocityType = CDVEL_EstimatedBody; + hasVelocityEstimatedBody = true; + velocityEstimatedBody = vel; + } + + void setDeltaTime(float time) + { + hasDeltaTime = true; + deltaTime = time; + } + + void setDeltaTheta(vec3f theta) + { + hasDeltaTheta = true; + deltaTheta = theta; + } + + void setDeltaVelocity(vec3f vel) + { + hasDeltaVelocity = true; + deltaVelocity = vel; + } + + void setTimeStartup(uint64_t ts) + { + hasTimeStartup = true; + timeStartup = ts; + } void setTimeGps(uint64_t time) { @@ -690,28 +676,28 @@ struct CompositeData::Impl } void setTimeSyncIn(uint64_t t) - { - hasTimeSyncIn = true; - timeSyncIn = t; - } - - void setVpeStatus(VpeStatus s) - { - hasVpeStatus = true; - vpeStatus = s; - } - - void setInsStatus(InsStatus s) - { - hasInsStatus = true; - insStatus = s; - } - - void setSyncInCnt(uint32_t count) - { - hasSyncInCnt = true; - syncInCnt = count; - } + { + hasTimeSyncIn = true; + timeSyncIn = t; + } + + void setVpeStatus(VpeStatus s) + { + hasVpeStatus = true; + vpeStatus = s; + } + + void setInsStatus(InsStatus s) + { + hasInsStatus = true; + insStatus = s; + } + + void setSyncInCnt(uint32_t count) + { + hasSyncInCnt = true; + syncInCnt = count; + } void setSyncOutCnt(uint32_t count) { @@ -746,7 +732,7 @@ struct CompositeData::Impl void setGpsTow(double tow) { hasGpsTow = true; - gpsTow = static_cast(tow*1e9); + gpsTow = static_cast(tow * 1e9); } void setGps2Tow(uint64_t tow) @@ -758,7 +744,7 @@ struct CompositeData::Impl void setGps2Tow(double tow) { hasGps2Tow = true; - gps2Tow = static_cast(tow*1e9); + gps2Tow = static_cast(tow * 1e9); } void setPositionUncertaintyGpsNed(vec3f u) @@ -776,11 +762,11 @@ struct CompositeData::Impl } void setPositionUncertaintyGpsEcef(vec3f u) - { - mostRecentlyUpdatedPositionUncertaintyType = CDPOU_GpsEcef; - hasPositionUncertaintyGpsEcef = true; - positionUncertaintyGpsEcef = u; - } + { + mostRecentlyUpdatedPositionUncertaintyType = CDPOU_GpsEcef; + hasPositionUncertaintyGpsEcef = true; + positionUncertaintyGpsEcef = u; + } void setPositionUncertaintyGps2Ecef(vec3f u) { @@ -790,11 +776,11 @@ struct CompositeData::Impl } void setPositionUncertaintyEstimated(float u) - { - mostRecentlyUpdatedPositionUncertaintyType = CDPOU_Estimated; - hasPositionUncertaintyEstimated = true; - positionUncertaintyEstimated = u; - } + { + mostRecentlyUpdatedPositionUncertaintyType = CDPOU_Estimated; + hasPositionUncertaintyEstimated = true; + positionUncertaintyEstimated = u; + } void setVelocityUncertaintyGps(float u) { @@ -811,11 +797,11 @@ struct CompositeData::Impl } void setVelocityUncertaintyEstimated(float u) - { - mostRecentlyUpdatedVelocityUncertaintyType = CDVEU_Estimated; - hasVelocityUncertaintyEstimated = true; - velocityUncertaintyEstimated = u; - } + { + mostRecentlyUpdatedVelocityUncertaintyType = CDVEU_Estimated; + hasVelocityUncertaintyEstimated = true; + velocityUncertaintyEstimated = u; + } void setTimeUncertainty(uint32_t u) { @@ -830,10 +816,10 @@ struct CompositeData::Impl } void setAttitudeUncertainty(vec3f u) - { - hasAttitudeUncertainty = true; - attitudeUncertainty = u; - } + { + hasAttitudeUncertainty = true; + attitudeUncertainty = u; + } void setFix(GpsFix f) { @@ -860,10 +846,10 @@ struct CompositeData::Impl } void setSensSat(SensSat s) - { - hasSensSat = true; - sensSat = s; - } + { + hasSensSat = true; + sensSat = s; + } void setGnssDop(GnssDop d) { @@ -890,1861 +876,1500 @@ struct CompositeData::Impl } }; +CompositeData::CompositeData() : _i(new Impl) { _i->reset(); } - -CompositeData::CompositeData() : -_i(new Impl) -{ - _i->reset(); -} - -CompositeData::CompositeData(const CompositeData& cd) : -_i(NULL) -{ - (*this) = cd; -} - +CompositeData::CompositeData(const CompositeData & cd) : _i(NULL) { (*this) = cd; } CompositeData::~CompositeData() { - if (NULL != _i) - { - delete _i; - _i = NULL; - } + if (NULL != _i) { + delete _i; + _i = NULL; + } } -CompositeData& CompositeData::operator=(const CompositeData& RHS) +CompositeData & CompositeData::operator=(const CompositeData & RHS) { - if (NULL != _i) - { - delete _i; - } + if (NULL != _i) { + delete _i; + } - _i = new Impl(); - (*_i) = (*RHS._i); + _i = new Impl(); + (*_i) = (*RHS._i); - return *this; + return *this; } -bool CompositeData::hasYawPitchRoll() -{ - return _i->hasYawPitchRoll; -} +bool CompositeData::hasYawPitchRoll() { return _i->hasYawPitchRoll; } vec3f CompositeData::yawPitchRoll() { - if (!hasYawPitchRoll()) - throw invalid_operation(); + if (!hasYawPitchRoll()) throw invalid_operation(); - return _i->yawPitchRoll; + return _i->yawPitchRoll; } -bool CompositeData::hasQuaternion() -{ - return _i->hasQuaternion; -} +bool CompositeData::hasQuaternion() { return _i->hasQuaternion; } vec4f CompositeData::quaternion() { - if (!hasQuaternion()) - throw invalid_operation(); + if (!hasQuaternion()) throw invalid_operation(); - return _i->quaternion; + return _i->quaternion; } -bool CompositeData::hasDirectionCosineMatrix() -{ - return _i->hasDirectionCosineMatrix; -} +bool CompositeData::hasDirectionCosineMatrix() { return _i->hasDirectionCosineMatrix; } mat3f CompositeData::directionCosineMatrix() { - if (!hasDirectionCosineMatrix()) - throw invalid_operation(); + if (!hasDirectionCosineMatrix()) throw invalid_operation(); - return _i->directionConsineMatrix; + return _i->directionConsineMatrix; } bool CompositeData::hasAnyMagnetic() { - return _i->mostRecentlyUpdatedMagneticType != Impl::CDMAG_None; + return _i->mostRecentlyUpdatedMagneticType != Impl::CDMAG_None; } vec3f CompositeData::anyMagnetic() { - switch (_i->mostRecentlyUpdatedMagneticType) - { - case Impl::CDMAG_None: - throw invalid_operation(); - case Impl::CDMAG_Normal: - return _i->magnetic; - case Impl::CDMAG_Uncompensated: - return _i->magneticUncompensated; - case Impl::CDMAG_Ned: - return _i->magneticNed; - case Impl::CDMAG_Ecef: - return _i->magneticEcef; - default: - throw not_implemented(); - } + switch (_i->mostRecentlyUpdatedMagneticType) { + case Impl::CDMAG_None: + throw invalid_operation(); + case Impl::CDMAG_Normal: + return _i->magnetic; + case Impl::CDMAG_Uncompensated: + return _i->magneticUncompensated; + case Impl::CDMAG_Ned: + return _i->magneticNed; + case Impl::CDMAG_Ecef: + return _i->magneticEcef; + default: + throw not_implemented(); + } } -bool CompositeData::hasMagnetic() -{ - return _i->hasMagnetic; -} +bool CompositeData::hasMagnetic() { return _i->hasMagnetic; } vec3f CompositeData::magnetic() { - if (!hasMagnetic()) - throw invalid_operation(); + if (!hasMagnetic()) throw invalid_operation(); - return _i->magnetic; + return _i->magnetic; } -bool CompositeData::hasMagneticUncompensated() -{ - return _i->hasMagneticUncompensated; -} +bool CompositeData::hasMagneticUncompensated() { return _i->hasMagneticUncompensated; } vec3f CompositeData::magneticUncompensated() { - if (!hasMagneticUncompensated()) - throw invalid_operation(); + if (!hasMagneticUncompensated()) throw invalid_operation(); - return _i->magneticUncompensated; + return _i->magneticUncompensated; } -bool CompositeData::hasMagneticNed() -{ - return _i->hasMagneticNed; -} +bool CompositeData::hasMagneticNed() { return _i->hasMagneticNed; } vec3f CompositeData::magneticNed() { - if (!hasMagneticNed()) - throw invalid_operation(); + if (!hasMagneticNed()) throw invalid_operation(); - return _i->magneticNed; + return _i->magneticNed; } -bool CompositeData::hasMagneticEcef() -{ - return _i->hasMagneticEcef; -} +bool CompositeData::hasMagneticEcef() { return _i->hasMagneticEcef; } vec3f CompositeData::magneticEcef() { - if (!hasMagneticEcef()) - throw invalid_operation(); + if (!hasMagneticEcef()) throw invalid_operation(); - return _i->magneticEcef; + return _i->magneticEcef; } - bool CompositeData::hasAnyAcceleration() { - return _i->mostRecentlyUpdatedAccelerationType != Impl::CDACC_None; + return _i->mostRecentlyUpdatedAccelerationType != Impl::CDACC_None; } vec3f CompositeData::anyAcceleration() { - switch (_i->mostRecentlyUpdatedAccelerationType) - { - case Impl::CDACC_None: - throw invalid_operation(); - case Impl::CDACC_Normal: - return _i->acceleration; - case Impl::CDACC_LinearBody: - return _i->accelerationLinearBody; - case Impl::CDACC_Uncompensated: - return _i->accelerationUncompensated; - case Impl::CDACC_LinearNed: - return _i->accelerationLinearNed; - case Impl::CDACC_Ned: - return _i->accelerationNed; - case Impl::CDACC_Ecef: - return _i->accelerationEcef; - case Impl::CDACC_LinearEcef: - return _i->accelerationLinearEcef; - default: - throw not_implemented(); - } -} - -bool CompositeData::hasAcceleration() -{ - return _i->hasAcceleration; + switch (_i->mostRecentlyUpdatedAccelerationType) { + case Impl::CDACC_None: + throw invalid_operation(); + case Impl::CDACC_Normal: + return _i->acceleration; + case Impl::CDACC_LinearBody: + return _i->accelerationLinearBody; + case Impl::CDACC_Uncompensated: + return _i->accelerationUncompensated; + case Impl::CDACC_LinearNed: + return _i->accelerationLinearNed; + case Impl::CDACC_Ned: + return _i->accelerationNed; + case Impl::CDACC_Ecef: + return _i->accelerationEcef; + case Impl::CDACC_LinearEcef: + return _i->accelerationLinearEcef; + default: + throw not_implemented(); + } } +bool CompositeData::hasAcceleration() { return _i->hasAcceleration; } + vec3f CompositeData::acceleration() { - if (!hasAcceleration()) - throw invalid_operation(); + if (!hasAcceleration()) throw invalid_operation(); - return _i->acceleration; + return _i->acceleration; } -bool CompositeData::hasAccelerationLinearBody() -{ - return _i->hasAccelerationLinearBody; -} +bool CompositeData::hasAccelerationLinearBody() { return _i->hasAccelerationLinearBody; } vec3f CompositeData::accelerationLinearBody() { - if (!hasAccelerationLinearBody()) - throw invalid_operation(); + if (!hasAccelerationLinearBody()) throw invalid_operation(); - return _i->accelerationLinearBody; + return _i->accelerationLinearBody; } -bool CompositeData::hasAccelerationUncompensated() -{ - return _i->hasAccelerationUncompensated; -} +bool CompositeData::hasAccelerationUncompensated() { return _i->hasAccelerationUncompensated; } vec3f CompositeData::accelerationUncompensated() { - if (!hasAccelerationUncompensated()) - throw invalid_operation(); + if (!hasAccelerationUncompensated()) throw invalid_operation(); - return _i->accelerationUncompensated; + return _i->accelerationUncompensated; } -bool CompositeData::hasAccelerationLinearNed() -{ - return _i->hasAccelerationLinearNed; -} +bool CompositeData::hasAccelerationLinearNed() { return _i->hasAccelerationLinearNed; } vec3f CompositeData::accelerationLinearNed() { - if (!hasAccelerationLinearNed()) - throw invalid_operation(); + if (!hasAccelerationLinearNed()) throw invalid_operation(); - return _i->accelerationLinearNed; + return _i->accelerationLinearNed; } -bool CompositeData::hasAccelerationLinearEcef() -{ - return _i->hasAccelerationLinearEcef; -} +bool CompositeData::hasAccelerationLinearEcef() { return _i->hasAccelerationLinearEcef; } vec3f CompositeData::accelerationLinearEcef() { - if (!hasAccelerationLinearEcef()) - throw invalid_operation(); + if (!hasAccelerationLinearEcef()) throw invalid_operation(); - return _i->accelerationLinearEcef; + return _i->accelerationLinearEcef; } -bool CompositeData::hasAccelerationNed() -{ - return _i->hasAccelerationNed; -} +bool CompositeData::hasAccelerationNed() { return _i->hasAccelerationNed; } vec3f CompositeData::accelerationNed() { - if (!hasAccelerationNed()) - throw invalid_operation(); + if (!hasAccelerationNed()) throw invalid_operation(); - return _i->accelerationNed; + return _i->accelerationNed; } -bool CompositeData::hasAccelerationEcef() -{ - return _i->hasAccelerationEcef; -} +bool CompositeData::hasAccelerationEcef() { return _i->hasAccelerationEcef; } vec3f CompositeData::accelerationEcef() { - if (!hasAccelerationEcef()) - throw invalid_operation(); + if (!hasAccelerationEcef()) throw invalid_operation(); - return _i->accelerationEcef; + return _i->accelerationEcef; } - bool CompositeData::hasAnyAngularRate() { - return _i->mostRecentlyUpdatedAngularRateType != Impl::CDANR_None; + return _i->mostRecentlyUpdatedAngularRateType != Impl::CDANR_None; } vec3f CompositeData::anyAngularRate() { - switch (_i->mostRecentlyUpdatedAngularRateType) - { - case Impl::CDANR_None: - throw invalid_operation(); - case Impl::CDANR_Normal: - return _i->angularRate; - case Impl::CDANR_Uncompensated: - return _i->angularRateUncompensated; - default: - throw not_implemented(); - } + switch (_i->mostRecentlyUpdatedAngularRateType) { + case Impl::CDANR_None: + throw invalid_operation(); + case Impl::CDANR_Normal: + return _i->angularRate; + case Impl::CDANR_Uncompensated: + return _i->angularRateUncompensated; + default: + throw not_implemented(); + } } -bool CompositeData::hasAngularRate() -{ - return _i->hasAngularRate; -} +bool CompositeData::hasAngularRate() { return _i->hasAngularRate; } vec3f CompositeData::angularRate() { - if (!hasAngularRate()) - throw invalid_operation(); + if (!hasAngularRate()) throw invalid_operation(); - return _i->angularRate; + return _i->angularRate; } -bool CompositeData::hasAngularRateUncompensated() -{ - return _i->hasAngularRateUncompensated; -} +bool CompositeData::hasAngularRateUncompensated() { return _i->hasAngularRateUncompensated; } vec3f CompositeData::angularRateUncompensated() { - if (!hasAngularRateUncompensated()) - throw invalid_operation(); + if (!hasAngularRateUncompensated()) throw invalid_operation(); - return _i->angularRateUncompensated; + return _i->angularRateUncompensated; } - bool CompositeData::hasAnyTemperature() { - return _i->mostRecentlyUpdatedTemperatureType != Impl::CDTEM_None; + return _i->mostRecentlyUpdatedTemperatureType != Impl::CDTEM_None; } float CompositeData::anyTemperature() { - switch (_i->mostRecentlyUpdatedTemperatureType) - { - case Impl::CDTEM_None: - throw invalid_operation(); - case Impl::CDTEM_Normal: - return _i->temperature; - default: - throw not_implemented(); - } + switch (_i->mostRecentlyUpdatedTemperatureType) { + case Impl::CDTEM_None: + throw invalid_operation(); + case Impl::CDTEM_Normal: + return _i->temperature; + default: + throw not_implemented(); + } } -bool CompositeData::hasTemperature() -{ - return _i->hasTemperature; -} +bool CompositeData::hasTemperature() { return _i->hasTemperature; } float CompositeData::temperature() { - if (!hasTemperature()) - throw invalid_operation(); + if (!hasTemperature()) throw invalid_operation(); - return _i->temperature; + return _i->temperature; } - bool CompositeData::hasAnyPressure() { - return _i->mostRecentlyUpdatePressureType != Impl::CDPRE_None; + return _i->mostRecentlyUpdatePressureType != Impl::CDPRE_None; } float CompositeData::anyPressure() { - switch (_i->mostRecentlyUpdatePressureType) - { - case Impl::CDPRE_None: - throw invalid_operation(); - case Impl::CDPRE_Normal: - return _i->pressure; - default: - throw not_implemented(); - } + switch (_i->mostRecentlyUpdatePressureType) { + case Impl::CDPRE_None: + throw invalid_operation(); + case Impl::CDPRE_Normal: + return _i->pressure; + default: + throw not_implemented(); + } } -bool CompositeData::hasPressure() -{ - return _i->hasPressure; -} +bool CompositeData::hasPressure() { return _i->hasPressure; } float CompositeData::pressure() { - if (!hasPressure()) - throw invalid_operation(); + if (!hasPressure()) throw invalid_operation(); - return _i->pressure; + return _i->pressure; } bool CompositeData::hasAnyPosition() { - return _i->mostRecentlyUpdatedPositionType != Impl::CDPOS_None; + return _i->mostRecentlyUpdatedPositionType != Impl::CDPOS_None; } PositionD CompositeData::anyPosition() { - switch (_i->mostRecentlyUpdatedPositionType) - { - case Impl::CDPOS_None: - throw invalid_operation(); - case Impl::CDPOS_GpsLla: - return PositionD::fromLla(_i->positionGpsLla); - case Impl::CDPOS_Gps2Lla: - return PositionD::fromLla(_i->positionGps2Lla); - case Impl::CDPOS_GpsEcef: - return PositionD::fromEcef(_i->positionGpsEcef); - case Impl::CDPOS_Gps2Ecef: - return PositionD::fromEcef(_i->positionGps2Ecef); - case Impl::CDPOS_EstimatedLla: - return PositionD::fromLla(_i->positionEstimatedLla); - case Impl::CDPOS_EstimatedEcef: - return PositionD::fromEcef(_i->positionEstimatedEcef); - default: - throw not_implemented(); - } + switch (_i->mostRecentlyUpdatedPositionType) { + case Impl::CDPOS_None: + throw invalid_operation(); + case Impl::CDPOS_GpsLla: + return PositionD::fromLla(_i->positionGpsLla); + case Impl::CDPOS_Gps2Lla: + return PositionD::fromLla(_i->positionGps2Lla); + case Impl::CDPOS_GpsEcef: + return PositionD::fromEcef(_i->positionGpsEcef); + case Impl::CDPOS_Gps2Ecef: + return PositionD::fromEcef(_i->positionGps2Ecef); + case Impl::CDPOS_EstimatedLla: + return PositionD::fromLla(_i->positionEstimatedLla); + case Impl::CDPOS_EstimatedEcef: + return PositionD::fromEcef(_i->positionEstimatedEcef); + default: + throw not_implemented(); + } } -bool CompositeData::hasPositionGpsLla() -{ - return _i->hasPositionGpsLla; -} +bool CompositeData::hasPositionGpsLla() { return _i->hasPositionGpsLla; } -bool CompositeData::hasPositionGps2Lla() -{ - return _i->hasPositionGps2Lla; -} +bool CompositeData::hasPositionGps2Lla() { return _i->hasPositionGps2Lla; } vec3d CompositeData::positionGpsLla() { - if(!hasPositionGpsLla()) - throw invalid_operation(); + if (!hasPositionGpsLla()) throw invalid_operation(); return _i->positionGpsLla; } vec3d CompositeData::positionGps2Lla() { - if(!hasPositionGps2Lla()) - throw invalid_operation(); + if (!hasPositionGps2Lla()) throw invalid_operation(); return _i->positionGps2Lla; } -bool CompositeData::hasPositionGpsEcef() -{ - return _i->hasPositionGpsEcef; -} +bool CompositeData::hasPositionGpsEcef() { return _i->hasPositionGpsEcef; } -bool CompositeData::hasPositionGps2Ecef() -{ - return _i->hasPositionGps2Ecef; -} +bool CompositeData::hasPositionGps2Ecef() { return _i->hasPositionGps2Ecef; } vec3d CompositeData::positionGpsEcef() { - if (!hasPositionGpsEcef()) - throw invalid_operation(); + if (!hasPositionGpsEcef()) throw invalid_operation(); - return _i->positionGpsEcef; + return _i->positionGpsEcef; } vec3d CompositeData::positionGps2Ecef() { - if (!hasPositionGps2Ecef()) - throw invalid_operation(); + if (!hasPositionGps2Ecef()) throw invalid_operation(); - return _i->positionGps2Ecef; + return _i->positionGps2Ecef; } -bool CompositeData::hasPositionEstimatedLla() -{ - return _i->hasPositionEstimatedLla; -} +bool CompositeData::hasPositionEstimatedLla() { return _i->hasPositionEstimatedLla; } vec3d CompositeData::positionEstimatedLla() { - if (!hasPositionEstimatedLla()) - throw invalid_operation(); + if (!hasPositionEstimatedLla()) throw invalid_operation(); - return _i->positionEstimatedLla; + return _i->positionEstimatedLla; } -bool CompositeData::hasPositionEstimatedEcef() -{ - return _i->hasPositionEstimatedEcef; -} +bool CompositeData::hasPositionEstimatedEcef() { return _i->hasPositionEstimatedEcef; } vec3d CompositeData::positionEstimatedEcef() { - if (!hasPositionEstimatedEcef()) - throw invalid_operation(); + if (!hasPositionEstimatedEcef()) throw invalid_operation(); - return _i->positionEstimatedEcef; + return _i->positionEstimatedEcef; } bool CompositeData::hasAnyVelocity() { - return _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_None; + return _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_None; } vec3f CompositeData::anyVelocity() { - switch (_i->mostRecentlyUpdatedVelocityType) - { - case Impl::CDVEL_None: - throw invalid_operation(); - case Impl::CDVEL_GpsNed: - return _i->velocityGpsNed; - case Impl::CDVEL_Gps2Ned: - return _i->velocityGps2Ned; - case Impl::CDVEL_GpsEcef: - return _i->velocityGpsEcef; - case Impl::CDVEL_Gps2Ecef: - return _i->velocityGps2Ecef; - case Impl::CDVEL_EstimatedNed: - return _i->velocityEstimatedNed; - case Impl::CDVEL_EstimatedEcef: - return _i->velocityEstimatedEcef; - case Impl::CDVEL_EstimatedBody: - return _i->velocityEstimatedBody; - default: - throw not_implemented(); - } + switch (_i->mostRecentlyUpdatedVelocityType) { + case Impl::CDVEL_None: + throw invalid_operation(); + case Impl::CDVEL_GpsNed: + return _i->velocityGpsNed; + case Impl::CDVEL_Gps2Ned: + return _i->velocityGps2Ned; + case Impl::CDVEL_GpsEcef: + return _i->velocityGpsEcef; + case Impl::CDVEL_Gps2Ecef: + return _i->velocityGps2Ecef; + case Impl::CDVEL_EstimatedNed: + return _i->velocityEstimatedNed; + case Impl::CDVEL_EstimatedEcef: + return _i->velocityEstimatedEcef; + case Impl::CDVEL_EstimatedBody: + return _i->velocityEstimatedBody; + default: + throw not_implemented(); + } } -bool CompositeData::hasVelocityGpsNed() -{ - return _i->hasVelocityGpsNed; -} +bool CompositeData::hasVelocityGpsNed() { return _i->hasVelocityGpsNed; } -bool CompositeData::hasVelocityGps2Ned() -{ - return _i->hasVelocityGps2Ned; -} +bool CompositeData::hasVelocityGps2Ned() { return _i->hasVelocityGps2Ned; } vec3f CompositeData::velocityGpsNed() { - if(!hasVelocityGpsNed()) - throw invalid_operation(); + if (!hasVelocityGpsNed()) throw invalid_operation(); return _i->velocityGpsNed; } vec3f CompositeData::velocityGps2Ned() { - if(!hasVelocityGps2Ned()) - throw invalid_operation(); + if (!hasVelocityGps2Ned()) throw invalid_operation(); return _i->velocityGps2Ned; } -bool CompositeData::hasVelocityGpsEcef() -{ - return _i->hasVelocityGpsEcef; -} +bool CompositeData::hasVelocityGpsEcef() { return _i->hasVelocityGpsEcef; } -bool CompositeData::hasVelocityGps2Ecef() -{ - return _i->hasVelocityGps2Ecef; -} +bool CompositeData::hasVelocityGps2Ecef() { return _i->hasVelocityGps2Ecef; } vec3f CompositeData::velocityGpsEcef() { - if(!hasVelocityGpsEcef()) - throw invalid_operation(); + if (!hasVelocityGpsEcef()) throw invalid_operation(); return _i->velocityGpsEcef; } vec3f CompositeData::velocityGps2Ecef() { - if(!hasVelocityGps2Ecef()) - throw invalid_operation(); + if (!hasVelocityGps2Ecef()) throw invalid_operation(); return _i->velocityGps2Ecef; } -bool CompositeData::hasVelocityEstimatedNed() -{ - return _i->hasVelocityEstimatedNed; -} +bool CompositeData::hasVelocityEstimatedNed() { return _i->hasVelocityEstimatedNed; } vec3f CompositeData::velocityEstimatedNed() { - if (!hasVelocityEstimatedNed()) - throw invalid_operation(); + if (!hasVelocityEstimatedNed()) throw invalid_operation(); - return _i->velocityEstimatedNed; + return _i->velocityEstimatedNed; } -bool CompositeData::hasVelocityEstimatedEcef() -{ - return _i->hasVelocityEstimatedEcef; -} +bool CompositeData::hasVelocityEstimatedEcef() { return _i->hasVelocityEstimatedEcef; } vec3f CompositeData::velocityEstimatedEcef() { - if (!hasVelocityEstimatedEcef()) - throw invalid_operation(); + if (!hasVelocityEstimatedEcef()) throw invalid_operation(); - return _i->velocityEstimatedEcef; + return _i->velocityEstimatedEcef; } -bool CompositeData::hasVelocityEstimatedBody() -{ - return _i->hasVelocityEstimatedBody; -} +bool CompositeData::hasVelocityEstimatedBody() { return _i->hasVelocityEstimatedBody; } vec3f CompositeData::velocityEstimatedBody() { - if (!hasVelocityEstimatedBody()) - throw invalid_operation(); + if (!hasVelocityEstimatedBody()) throw invalid_operation(); - return _i->velocityEstimatedBody; + return _i->velocityEstimatedBody; } -bool CompositeData::hasDeltaTime() -{ - return _i->hasDeltaTime; -} +bool CompositeData::hasDeltaTime() { return _i->hasDeltaTime; } float CompositeData::deltaTime() { - if (!hasDeltaTime()) - throw invalid_operation(); + if (!hasDeltaTime()) throw invalid_operation(); - return _i->deltaTime; + return _i->deltaTime; } -bool CompositeData::hasDeltaTheta() -{ - return _i->hasDeltaTheta; -} +bool CompositeData::hasDeltaTheta() { return _i->hasDeltaTheta; } vec3f CompositeData::deltaTheta() { - if (!hasDeltaTheta()) - throw invalid_operation(); + if (!hasDeltaTheta()) throw invalid_operation(); - return _i->deltaTheta; + return _i->deltaTheta; } -bool CompositeData::hasDeltaVelocity() -{ - return _i->hasDeltaVelocity; -} +bool CompositeData::hasDeltaVelocity() { return _i->hasDeltaVelocity; } vec3f CompositeData::deltaVelocity() { - if (!hasDeltaVelocity()) - throw invalid_operation(); + if (!hasDeltaVelocity()) throw invalid_operation(); - return _i->deltaVelocity; + return _i->deltaVelocity; } -bool CompositeData::hasTimeStartup() -{ - return _i->hasTimeStartup; -} +bool CompositeData::hasTimeStartup() { return _i->hasTimeStartup; } uint64_t CompositeData::timeStartup() { - if (!hasTimeStartup()) - throw invalid_operation(); + if (!hasTimeStartup()) throw invalid_operation(); - return _i->timeStartup; + return _i->timeStartup; } -bool CompositeData::hasTimeGps() -{ - return _i->hasTimeGps; -} +bool CompositeData::hasTimeGps() { return _i->hasTimeGps; } -bool CompositeData::hasTimeGps2() -{ - return _i->hasTimeGps2; -} +bool CompositeData::hasTimeGps2() { return _i->hasTimeGps2; } uint64_t CompositeData::timeGps() { - if(!hasTimeGps()) - throw invalid_operation(); + if (!hasTimeGps()) throw invalid_operation(); return _i->timeGps; } uint64_t CompositeData::timeGps2() { - if(!hasTimeGps2()) - throw invalid_operation(); + if (!hasTimeGps2()) throw invalid_operation(); return _i->timeGps2; } -bool CompositeData::hasTow() -{ - return _i->hasTow; -} +bool CompositeData::hasTow() { return _i->hasTow; } double CompositeData::tow() { - if(!hasTow()) - throw invalid_operation(); + if (!hasTow()) throw invalid_operation(); return _i->tow; } -bool CompositeData::hasWeek() -{ - return _i->hasWeek; -} +bool CompositeData::hasWeek() { return _i->hasWeek; } uint16_t CompositeData::week() { - if (!hasWeek()) - throw invalid_operation(); + if (!hasWeek()) throw invalid_operation(); - return _i->week; + return _i->week; } -bool CompositeData::hasNumSats() -{ - return _i->hasNumSats; -} +bool CompositeData::hasNumSats() { return _i->hasNumSats; } uint8_t CompositeData::numSats() { - if (!hasNumSats()) - throw invalid_operation(); + if (!hasNumSats()) throw invalid_operation(); - return _i->numSats; + return _i->numSats; } -bool CompositeData::hasTimeSyncIn() -{ - return _i->hasTimeSyncIn; -} +bool CompositeData::hasTimeSyncIn() { return _i->hasTimeSyncIn; } uint64_t CompositeData::timeSyncIn() { - if (!hasTimeSyncIn()) - throw invalid_operation(); + if (!hasTimeSyncIn()) throw invalid_operation(); - return _i->timeSyncIn; + return _i->timeSyncIn; } -bool CompositeData::hasVpeStatus() -{ - return _i->hasVpeStatus; -} +bool CompositeData::hasVpeStatus() { return _i->hasVpeStatus; } VpeStatus CompositeData::vpeStatus() { - if (!hasVpeStatus()) - throw invalid_operation(); + if (!hasVpeStatus()) throw invalid_operation(); - return _i->vpeStatus; + return _i->vpeStatus; } -bool CompositeData::hasInsStatus() -{ - return _i->hasInsStatus; -} +bool CompositeData::hasInsStatus() { return _i->hasInsStatus; } InsStatus CompositeData::insStatus() { - if (!hasInsStatus()) - throw invalid_operation(); + if (!hasInsStatus()) throw invalid_operation(); - return _i->insStatus; + return _i->insStatus; } -bool CompositeData::hasSyncInCnt() -{ - return _i->hasSyncInCnt; -} +bool CompositeData::hasSyncInCnt() { return _i->hasSyncInCnt; } uint32_t CompositeData::syncInCnt() { - if (!hasSyncInCnt()) - throw invalid_operation(); + if (!hasSyncInCnt()) throw invalid_operation(); - return _i->syncInCnt; + return _i->syncInCnt; } -bool CompositeData::hasSyncOutCnt() -{ - return _i->hasSyncOutCnt; -} +bool CompositeData::hasSyncOutCnt() { return _i->hasSyncOutCnt; } uint32_t CompositeData::syncOutCnt() { - if (!hasSyncOutCnt()) - throw invalid_operation(); + if (!hasSyncOutCnt()) throw invalid_operation(); return _i->syncOutCnt; } -bool CompositeData::hasTimeStatus() -{ - return _i->hasTimeStatus; -} +bool CompositeData::hasTimeStatus() { return _i->hasTimeStatus; } uint8_t CompositeData::timeStatus() { - if (!hasTimeStatus()) - throw invalid_operation(); + if (!hasTimeStatus()) throw invalid_operation(); return _i->timeStatus; } -bool CompositeData::hasTimeGpsPps() -{ - return _i->hasTimeGpsPps; -} +bool CompositeData::hasTimeGpsPps() { return _i->hasTimeGpsPps; } -bool CompositeData::hasTimeGps2Pps() -{ - return _i->hasTimeGps2Pps; -} +bool CompositeData::hasTimeGps2Pps() { return _i->hasTimeGps2Pps; } uint64_t CompositeData::timeGpsPps() { - if (!hasTimeGpsPps()) - throw invalid_operation(); + if (!hasTimeGpsPps()) throw invalid_operation(); - return _i->timeGpsPps; + return _i->timeGpsPps; } uint64_t CompositeData::timeGps2Pps() { - if(!hasTimeGps2Pps()) - throw invalid_operation(); + if (!hasTimeGps2Pps()) throw invalid_operation(); return _i->timeGps2Pps; } -bool CompositeData::hasGpsTow() -{ - return _i->hasGpsTow; -} +bool CompositeData::hasGpsTow() { return _i->hasGpsTow; } -bool CompositeData::hasGps2Tow() -{ - return _i->hasGps2Tow; -} +bool CompositeData::hasGps2Tow() { return _i->hasGps2Tow; } uint64_t CompositeData::gpsTow() { - if(!hasGpsTow()) - throw invalid_operation(); + if (!hasGpsTow()) throw invalid_operation(); return _i->gpsTow; } uint64_t CompositeData::gps2Tow() { - if(!hasGps2Tow()) - throw invalid_operation(); + if (!hasGps2Tow()) throw invalid_operation(); return _i->gps2Tow; } -bool CompositeData::hasTimeUtc() -{ - return _i->hasTimeUtc; -} +bool CompositeData::hasTimeUtc() { return _i->hasTimeUtc; } TimeUtc CompositeData::timeUtc() { - if (!hasTimeUtc()) - throw invalid_operation(); + if (!hasTimeUtc()) throw invalid_operation(); - return _i->timeUtc; + return _i->timeUtc; } -bool CompositeData::hasSensSat() -{ - return _i->hasSensSat; -} +bool CompositeData::hasSensSat() { return _i->hasSensSat; } SensSat CompositeData::sensSat() { - if (!hasSensSat()) - throw invalid_operation(); + if (!hasSensSat()) throw invalid_operation(); - return _i->sensSat; + return _i->sensSat; } -bool CompositeData::hasFix() -{ - return _i->hasFix; -} +bool CompositeData::hasFix() { return _i->hasFix; } -bool CompositeData::hasFix2() -{ - return _i->hasFix2; -} +bool CompositeData::hasFix2() { return _i->hasFix2; } GpsFix CompositeData::fix() { - if(!hasFix()) - throw invalid_operation(); + if (!hasFix()) throw invalid_operation(); return _i->fix; } GpsFix CompositeData::fix2() { - if(!hasFix2()) - throw invalid_operation(); + if (!hasFix2()) throw invalid_operation(); return _i->fix2; } bool CompositeData::hasAnyPositionUncertainty() { - return _i->mostRecentlyUpdatedPositionUncertaintyType != Impl::CDPOU_None; + return _i->mostRecentlyUpdatedPositionUncertaintyType != Impl::CDPOU_None; } vec3f CompositeData::anyPositionUncertainty() { - switch (_i->mostRecentlyUpdatedPositionUncertaintyType) - { - case Impl::CDPOU_None: - throw invalid_operation(); - case Impl::CDPOU_GpsNed: - return _i->positionUncertaintyGpsNed; - case Impl::CDPOU_Gps2Ned: - return _i->positionUncertaintyGps2Ned; - case Impl::CDPOU_GpsEcef: - return _i->positionUncertaintyGpsEcef; - case Impl::CDPOU_Gps2Ecef: - return _i->positionUncertaintyGps2Ecef; - case Impl::CDPOU_Estimated: - return vec3f(_i->positionUncertaintyEstimated); - default: - throw not_implemented(); - } + switch (_i->mostRecentlyUpdatedPositionUncertaintyType) { + case Impl::CDPOU_None: + throw invalid_operation(); + case Impl::CDPOU_GpsNed: + return _i->positionUncertaintyGpsNed; + case Impl::CDPOU_Gps2Ned: + return _i->positionUncertaintyGps2Ned; + case Impl::CDPOU_GpsEcef: + return _i->positionUncertaintyGpsEcef; + case Impl::CDPOU_Gps2Ecef: + return _i->positionUncertaintyGps2Ecef; + case Impl::CDPOU_Estimated: + return vec3f(_i->positionUncertaintyEstimated); + default: + throw not_implemented(); + } } -bool CompositeData::hasPositionUncertaintyGpsNed() -{ - return _i->hasPositionUncertaintyGpsNed; -} +bool CompositeData::hasPositionUncertaintyGpsNed() { return _i->hasPositionUncertaintyGpsNed; } -bool CompositeData::hasPositionUncertaintyGps2Ned() -{ - return _i->hasPositionUncertaintyGps2Ned; -} +bool CompositeData::hasPositionUncertaintyGps2Ned() { return _i->hasPositionUncertaintyGps2Ned; } vec3f CompositeData::positionUncertaintyGpsNed() { - if(!hasPositionUncertaintyGpsNed()) - throw invalid_operation(); + if (!hasPositionUncertaintyGpsNed()) throw invalid_operation(); return _i->positionUncertaintyGpsNed; } vec3f CompositeData::positionUncertaintyGps2Ned() { - if(!hasPositionUncertaintyGps2Ned()) - throw invalid_operation(); + if (!hasPositionUncertaintyGps2Ned()) throw invalid_operation(); return _i->positionUncertaintyGps2Ned; } -bool CompositeData::hasPositionUncertaintyGpsEcef() -{ - return _i->hasPositionUncertaintyGpsEcef; -} +bool CompositeData::hasPositionUncertaintyGpsEcef() { return _i->hasPositionUncertaintyGpsEcef; } -bool CompositeData::hasPositionUncertaintyGps2Ecef() -{ - return _i->hasPositionUncertaintyGps2Ecef; -} +bool CompositeData::hasPositionUncertaintyGps2Ecef() { return _i->hasPositionUncertaintyGps2Ecef; } vec3f CompositeData::positionUncertaintyGpsEcef() { - if(!hasPositionUncertaintyGpsEcef()) - throw invalid_operation(); + if (!hasPositionUncertaintyGpsEcef()) throw invalid_operation(); return _i->positionUncertaintyGpsEcef; } vec3f CompositeData::positionUncertaintyGps2Ecef() { - if(!hasPositionUncertaintyGps2Ecef()) - throw invalid_operation(); + if (!hasPositionUncertaintyGps2Ecef()) throw invalid_operation(); return _i->positionUncertaintyGps2Ecef; } bool CompositeData::hasPositionUncertaintyEstimated() { - return _i->hasPositionUncertaintyEstimated; + return _i->hasPositionUncertaintyEstimated; } float CompositeData::positionUncertaintyEstimated() { - if (!hasPositionUncertaintyEstimated()) - throw invalid_operation(); + if (!hasPositionUncertaintyEstimated()) throw invalid_operation(); - return _i->positionUncertaintyEstimated; + return _i->positionUncertaintyEstimated; } bool CompositeData::hasAnyVelocityUncertainty() { - return _i->mostRecentlyUpdatedVelocityUncertaintyType != Impl::CDVEU_None; + return _i->mostRecentlyUpdatedVelocityUncertaintyType != Impl::CDVEU_None; } float CompositeData::anyVelocityUncertainty() { - switch (_i->mostRecentlyUpdatedVelocityUncertaintyType) - { - case Impl::CDVEU_None: - throw invalid_operation(); - case Impl::CDVEU_Gps: - return _i->velocityUncertaintyGps; - case Impl::CDVEU_Gps2: - return _i->velocityUncertaintyGps2; - case Impl::CDVEU_Estimated: - return _i->velocityUncertaintyEstimated; - default: - throw not_implemented(); - } + switch (_i->mostRecentlyUpdatedVelocityUncertaintyType) { + case Impl::CDVEU_None: + throw invalid_operation(); + case Impl::CDVEU_Gps: + return _i->velocityUncertaintyGps; + case Impl::CDVEU_Gps2: + return _i->velocityUncertaintyGps2; + case Impl::CDVEU_Estimated: + return _i->velocityUncertaintyEstimated; + default: + throw not_implemented(); + } } -bool CompositeData::hasVelocityUncertaintyGps() -{ - return _i->hasVelocityUncertaintyGps; -} +bool CompositeData::hasVelocityUncertaintyGps() { return _i->hasVelocityUncertaintyGps; } -bool CompositeData::hasVelocityUncertaintyGps2() -{ - return _i->hasVelocityUncertaintyGps2; -} +bool CompositeData::hasVelocityUncertaintyGps2() { return _i->hasVelocityUncertaintyGps2; } float CompositeData::velocityUncertaintyGps() { - if (!hasVelocityUncertaintyGps()) - throw invalid_operation(); + if (!hasVelocityUncertaintyGps()) throw invalid_operation(); - return _i->velocityUncertaintyGps; + return _i->velocityUncertaintyGps; } float CompositeData::velocityUncertaintyGps2() { - if(!hasVelocityUncertaintyGps2()) - throw invalid_operation(); + if (!hasVelocityUncertaintyGps2()) throw invalid_operation(); return _i->velocityUncertaintyGps2; } bool CompositeData::hasVelocityUncertaintyEstimated() { - return _i->hasVelocityUncertaintyEstimated; + return _i->hasVelocityUncertaintyEstimated; } float CompositeData::velocityUncertaintyEstimated() { - if (!hasVelocityUncertaintyEstimated()) - throw invalid_operation(); + if (!hasVelocityUncertaintyEstimated()) throw invalid_operation(); - return _i->velocityUncertaintyEstimated; + return _i->velocityUncertaintyEstimated; } -bool CompositeData::hasTimeUncertainty() -{ - return _i->hasTimeUncertainty; -} +bool CompositeData::hasTimeUncertainty() { return _i->hasTimeUncertainty; } uint32_t CompositeData::timeUncertainty() { - if (!hasTimeUncertainty()) - throw invalid_operation(); + if (!hasTimeUncertainty()) throw invalid_operation(); - return _i->timeUncertainty; + return _i->timeUncertainty; } -bool CompositeData::hasAttitudeUncertainty() -{ - return _i->hasAttitudeUncertainty; -} +bool CompositeData::hasAttitudeUncertainty() { return _i->hasAttitudeUncertainty; } vec3f CompositeData::attitudeUncertainty() { - if (!hasAttitudeUncertainty()) - throw invalid_operation(); + if (!hasAttitudeUncertainty()) throw invalid_operation(); - return _i->attitudeUncertainty; + return _i->attitudeUncertainty; } bool CompositeData::hasCourseOverGround() { - return _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_None - && _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_EstimatedBody // TODO: Don't have conversion formula from body frame to NED frame. - && _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_EstimatedEcef // TODO: Don't have conversion formula from ECEF frame to NED frame. - && _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_GpsEcef // TODO: Don't have conversion formula from ECEF frame to NED frame. - && _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_Gps2Ecef; // TODO: Don't have conversion formula from ECEF frame to NED frame. + return _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_None && + _i->mostRecentlyUpdatedVelocityType != + Impl:: + CDVEL_EstimatedBody // TODO: Don't have conversion formula from body frame to NED frame. + && + _i->mostRecentlyUpdatedVelocityType != + Impl:: + CDVEL_EstimatedEcef // TODO: Don't have conversion formula from ECEF frame to NED frame. + && + _i->mostRecentlyUpdatedVelocityType != + Impl::CDVEL_GpsEcef // TODO: Don't have conversion formula from ECEF frame to NED frame. + && + _i->mostRecentlyUpdatedVelocityType != + Impl:: + CDVEL_Gps2Ecef; // TODO: Don't have conversion formula from ECEF frame to NED frame. } float CompositeData::courseOverGround() { - if (!hasCourseOverGround()) - throw invalid_operation(); + if (!hasCourseOverGround()) throw invalid_operation(); - switch (_i->mostRecentlyUpdatedVelocityType) - { + switch (_i->mostRecentlyUpdatedVelocityType) { case Impl::CDVEL_GpsNed: return course_over_ground(_i->velocityGpsNed); case Impl::CDVEL_Gps2Ned: return course_over_ground(_i->velocityGps2Ned); case Impl::CDVEL_EstimatedNed: - return course_over_ground(_i->velocityEstimatedNed); - default: - throw not_implemented(); - } + return course_over_ground(_i->velocityEstimatedNed); + default: + throw not_implemented(); + } } bool CompositeData::hasSpeedOverGround() { - return _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_None - && _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_EstimatedBody // TODO: Don't have conversion formula from body frame to NED frame. - && _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_EstimatedEcef // TODO: Don't have conversion formula from ECEF frame to NED frame. - && _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_GpsEcef // TODO: Don't have conversion formula from ECEF frame to NED frame. - && _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_Gps2Ecef; // TODO: Don't have conversion formula from ECEF frame to NED frame. + return _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_None && + _i->mostRecentlyUpdatedVelocityType != + Impl:: + CDVEL_EstimatedBody // TODO: Don't have conversion formula from body frame to NED frame. + && + _i->mostRecentlyUpdatedVelocityType != + Impl:: + CDVEL_EstimatedEcef // TODO: Don't have conversion formula from ECEF frame to NED frame. + && + _i->mostRecentlyUpdatedVelocityType != + Impl::CDVEL_GpsEcef // TODO: Don't have conversion formula from ECEF frame to NED frame. + && + _i->mostRecentlyUpdatedVelocityType != + Impl:: + CDVEL_Gps2Ecef; // TODO: Don't have conversion formula from ECEF frame to NED frame. } float CompositeData::speedOverGround() { - if (!hasSpeedOverGround()) - throw invalid_operation(); + if (!hasSpeedOverGround()) throw invalid_operation(); - switch (_i->mostRecentlyUpdatedVelocityType) - { + switch (_i->mostRecentlyUpdatedVelocityType) { case Impl::CDVEL_GpsNed: return speed_over_ground(_i->velocityGpsNed); case Impl::CDVEL_Gps2Ned: return speed_over_ground(_i->velocityGps2Ned); case Impl::CDVEL_EstimatedNed: - return speed_over_ground(_i->velocityEstimatedNed); - default: - throw not_implemented(); - } + return speed_over_ground(_i->velocityEstimatedNed); + default: + throw not_implemented(); + } } -bool CompositeData::hasTimeInfo() -{ - return _i->hasTimeInfo; -} +bool CompositeData::hasTimeInfo() { return _i->hasTimeInfo; } TimeInfo CompositeData::timeInfo() { - if (!hasTimeInfo()) - throw invalid_operation(); + if (!hasTimeInfo()) throw invalid_operation(); return _i->timeInfo; } -bool CompositeData::hasDop() -{ - return _i->hasDop; -} +bool CompositeData::hasDop() { return _i->hasDop; } GnssDop CompositeData::dop() { - if (!hasDop()) - throw invalid_operation(); + if (!hasDop()) throw invalid_operation(); return _i->dop; } -CompositeData CompositeData::parse(Packet& p) +CompositeData CompositeData::parse(Packet & p) { - CompositeData o; + CompositeData o; - parse(p, o); + parse(p, o); - return o; + return o; } -void CompositeData::parse(Packet& p, CompositeData& o) +void CompositeData::parse(Packet & p, CompositeData & o) { - vector v; - v.push_back(&o); + vector v; + v.push_back(&o); - parse(p, v); + parse(p, v); } -void CompositeData::parse(Packet& p, vector& o) +void CompositeData::parse(Packet & p, vector & o) { - if (p.type() == Packet::TYPE_ASCII) - parseAscii(p, o); - else if (p.type() == Packet::TYPE_BINARY) - parseBinary(p, o); - else - throw not_supported(); + if (p.type() == Packet::TYPE_ASCII) + parseAscii(p, o); + else if (p.type() == Packet::TYPE_BINARY) + parseBinary(p, o); + else + throw not_supported(); } -void CompositeData::reset() -{ - _i->reset(); -} +void CompositeData::reset() { _i->reset(); } bool CompositeData::hasAnyAttitude() { - return _i->mostRecentlyUpdatedAttitudeType != Impl::CDATT_None; + return _i->mostRecentlyUpdatedAttitudeType != Impl::CDATT_None; } AttitudeF CompositeData::anyAttitude() { - switch (_i->mostRecentlyUpdatedAttitudeType) - { - case Impl::CDATT_None: - throw invalid_operation("no attitude data present"); - case Impl::CDATT_YawPitchRoll: - return AttitudeF::fromYprInDegs(_i->yawPitchRoll); - case Impl::CDATT_Quaternion: - return AttitudeF::fromQuat(_i->quaternion); - case Impl::CDATT_DirectionCosineMatrix: - return AttitudeF::fromDcm(_i->directionConsineMatrix); - default: - throw not_implemented(); - } + switch (_i->mostRecentlyUpdatedAttitudeType) { + case Impl::CDATT_None: + throw invalid_operation("no attitude data present"); + case Impl::CDATT_YawPitchRoll: + return AttitudeF::fromYprInDegs(_i->yawPitchRoll); + case Impl::CDATT_Quaternion: + return AttitudeF::fromQuat(_i->quaternion); + case Impl::CDATT_DirectionCosineMatrix: + return AttitudeF::fromDcm(_i->directionConsineMatrix); + default: + throw not_implemented(); + } } -void CompositeData::parseAscii(Packet& p, vector& o) +void CompositeData::parseAscii(Packet & p, vector & o) { - switch (p.determineAsciiAsyncType()) - { + switch (p.determineAsciiAsyncType()) { + case VNYPR: { + vec3f ypr; - case VNYPR: - { - vec3f ypr; + p.parseVNYPR(&ypr); - p.parseVNYPR(&ypr); + for (cditer i = o.begin(); i != o.end(); ++i) (*i)->_i->setYawPitchRoll(ypr); - for (cditer i = o.begin(); i != o.end(); ++i) - (*i)->_i->setYawPitchRoll(ypr); + break; + } - break; - } + case VNQTN: { + vec4f quat; - case VNQTN: - { - vec4f quat; + p.parseVNQTN(&quat); - p.parseVNQTN(&quat); + for (cditer i = o.begin(); i != o.end(); ++i) (*i)->_i->setQuaternion(quat); - for (cditer i = o.begin(); i != o.end(); ++i) - (*i)->_i->setQuaternion(quat); + break; + } - break; - } + case VNQMR: { + vec4f quat; + vec3f mag, accel, ar; + p.parseVNQMR(&quat, &mag, &accel, &ar); - case VNQMR: - { - vec4f quat; - vec3f mag, accel, ar; + for (cditer i = o.begin(); i != o.end(); ++i) { + (*i)->_i->setQuaternion(quat); + (*i)->_i->setMagnetic(mag); + (*i)->_i->setAcceleration(accel); + (*i)->_i->setAngularRate(ar); + } - p.parseVNQMR(&quat, &mag, &accel, &ar); + break; + } - for (cditer i = o.begin(); i != o.end(); ++i) - { - (*i)->_i->setQuaternion(quat); - (*i)->_i->setMagnetic(mag); - (*i)->_i->setAcceleration(accel); - (*i)->_i->setAngularRate(ar); - } + case VNMAG: { + vec3f mag; - break; - } + p.parseVNMAG(&mag); + for (cditer i = o.begin(); i != o.end(); ++i) (*i)->_i->setMagnetic(mag); - case VNMAG: - { - vec3f mag; + break; + } - p.parseVNMAG(&mag); + case VNACC: { + vec3f accel; - for (cditer i = o.begin(); i != o.end(); ++i) - (*i)->_i->setMagnetic(mag); + p.parseVNACC(&accel); - break; - } + for (cditer i = o.begin(); i != o.end(); ++i) (*i)->_i->setAcceleration(accel); - case VNACC: - { - vec3f accel; + break; + } - p.parseVNACC(&accel); + case VNGYR: { + vec3f ar; - for (cditer i = o.begin(); i != o.end(); ++i) - (*i)->_i->setAcceleration(accel); + p.parseVNGYR(&ar); - break; - } + for (cditer i = o.begin(); i != o.end(); ++i) (*i)->_i->setAngularRate(ar); - case VNGYR: - { - vec3f ar; + break; + } - p.parseVNGYR(&ar); + case VNMAR: { + vec3f mag, accel, ar; - for (cditer i = o.begin(); i != o.end(); ++i) - (*i)->_i->setAngularRate(ar); + p.parseVNMAR(&mag, &accel, &ar); - break; - } + for (cditer i = o.begin(); i != o.end(); ++i) { + (*i)->_i->setMagnetic(mag); + (*i)->_i->setAcceleration(accel); + (*i)->_i->setAngularRate(ar); + } - case VNMAR: - { - vec3f mag, accel, ar; + break; + } - p.parseVNMAR(&mag, &accel, &ar); + case VNYMR: { + vec3f ypr, mag, accel, ar; - for (cditer i = o.begin(); i != o.end(); ++i) - { - (*i)->_i->setMagnetic(mag); - (*i)->_i->setAcceleration(accel); - (*i)->_i->setAngularRate(ar); - } + p.parseVNYMR(&ypr, &mag, &accel, &ar); - break; - } + for (cditer i = o.begin(); i != o.end(); ++i) { + (*i)->_i->setYawPitchRoll(ypr); + (*i)->_i->setMagnetic(mag); + (*i)->_i->setAcceleration(accel); + (*i)->_i->setAngularRate(ar); + } - case VNYMR: - { - vec3f ypr, mag, accel, ar; + break; + } - p.parseVNYMR(&ypr, &mag, &accel, &ar); + case VNYBA: { + vec3f ypr, accel, ar; - for (cditer i = o.begin(); i != o.end(); ++i) - { - (*i)->_i->setYawPitchRoll(ypr); - (*i)->_i->setMagnetic(mag); - (*i)->_i->setAcceleration(accel); - (*i)->_i->setAngularRate(ar); - } + p.parseVNYBA(&ypr, &accel, &ar); - break; - } + for (cditer i = o.begin(); i != o.end(); ++i) { + (*i)->_i->setYawPitchRoll(ypr); + (*i)->_i->setAccelerationLinearBody(accel); + (*i)->_i->setAngularRate(ar); + } + break; + } - case VNYBA: - { - vec3f ypr, accel, ar; + case VNYIA: { + vec3f ypr, accel, ar; - p.parseVNYBA(&ypr, &accel, &ar); + p.parseVNYIA(&ypr, &accel, &ar); - for (cditer i = o.begin(); i != o.end(); ++i) - { - (*i)->_i->setYawPitchRoll(ypr); - (*i)->_i->setAccelerationLinearBody(accel); - (*i)->_i->setAngularRate(ar); - } + for (cditer i = o.begin(); i != o.end(); ++i) { + (*i)->_i->setYawPitchRoll(ypr); + (*i)->_i->setAccelerationLinearNed(accel); + (*i)->_i->setAngularRate(ar); + } - break; - } + break; + } - case VNYIA: - { - vec3f ypr, accel, ar; + case VNIMU: { + vec3f accel, ar, mag; + float temp, pres; - p.parseVNYIA(&ypr, &accel, &ar); + p.parseVNIMU(&mag, &accel, &ar, &temp, &pres); - for (cditer i = o.begin(); i != o.end(); ++i) - { - (*i)->_i->setYawPitchRoll(ypr); - (*i)->_i->setAccelerationLinearNed(accel); - (*i)->_i->setAngularRate(ar); - } + for (cditer i = o.begin(); i != o.end(); ++i) { + (*i)->_i->setMagneticUncompensated(mag); + (*i)->_i->setAccelerationUncompensated(accel); + (*i)->_i->setAngularRateUncompensated(ar); + (*i)->_i->setTemperature(temp); + (*i)->_i->setPressure(pres); + } - break; - } + break; + } + case VNGPS: { + double time; + uint16_t week; + uint8_t fix, numSats; + vec3d lla; + vec3f nedVel, nedAcc; + float speedAcc, timeAcc; + + p.parseVNGPS(&time, &week, &fix, &numSats, &lla, &nedVel, &nedAcc, &speedAcc, &timeAcc); + + for (cditer i = o.begin(); i != o.end(); ++i) { + (*i)->_i->setGpsTow(time); + (*i)->_i->setGpsWeek(week); + (*i)->_i->setFix(static_cast(fix)); + (*i)->_i->setNumSats(numSats); + (*i)->_i->setPositionGpsLla(lla); + (*i)->_i->setVelocityGpsNed(nedVel); + (*i)->_i->setPositionUncertaintyGpsNed(nedAcc); + (*i)->_i->setVelocityUncertaintyGps(speedAcc); + // Convert to uint32_t since this is the binary representation in nanoseconds. + (*i)->_i->setTimeUncertainty(static_cast(timeAcc * 1e9)); + } + + break; + } - case VNIMU: - { - vec3f accel, ar, mag; - float temp, pres; + case VNG2S: { + double time; + uint16_t week; + uint8_t fix, numSats; + vec3d lla; + vec3f nedVel, nedAcc; + float speedAcc, timeAcc; + + p.parseVNGPS(&time, &week, &fix, &numSats, &lla, &nedVel, &nedAcc, &speedAcc, &timeAcc); + + for (cditer i = o.begin(); i != o.end(); ++i) { + (*i)->_i->setGps2Tow(time); + (*i)->_i->setGps2Week(week); + (*i)->_i->setFix2(static_cast(fix)); + (*i)->_i->setNumSats2(numSats); + (*i)->_i->setPositionGps2Lla(lla); + (*i)->_i->setVelocityGps2Ned(nedVel); + (*i)->_i->setPositionUncertaintyGps2Ned(nedAcc); + (*i)->_i->setVelocityUncertaintyGps2(speedAcc); + // Convert to uint32_t since this is the binary representation in nanoseconds. + (*i)->_i->setTimeUncertainty2(static_cast(timeAcc * 1e9)); + } + + break; + } - p.parseVNIMU(&mag, &accel, &ar, &temp, &pres); + case VNGPE: { + double tow; + uint16_t week; + uint8_t fix, numSats; + vec3d position; + vec3f ecefVel, ecefAcc; + float speedAcc, timeAcc; + + p.parseVNGPE(&tow, &week, &fix, &numSats, &position, &ecefVel, &ecefAcc, &speedAcc, &timeAcc); + + for (cditer i = o.begin(); i != o.end(); ++i) { + (*i)->_i->setGpsTow(tow); + (*i)->_i->setGpsWeek(week); + (*i)->_i->setFix(static_cast(fix)); + (*i)->_i->setNumSats(numSats); + (*i)->_i->setPositionGpsEcef(position); + (*i)->_i->setVelocityGpsEcef(ecefVel); + (*i)->_i->setPositionUncertaintyGpsEcef(ecefAcc); + (*i)->_i->setVelocityUncertaintyGps(speedAcc); + // Convert to uint32_t since this is the binary representation in nanoseconds. + (*i)->_i->setTimeUncertainty(static_cast(timeAcc * 1e9)); + } + + break; + } - for (cditer i = o.begin(); i != o.end(); ++i) - { - (*i)->_i->setMagneticUncompensated(mag); - (*i)->_i->setAccelerationUncompensated(accel); - (*i)->_i->setAngularRateUncompensated(ar); - (*i)->_i->setTemperature(temp); - (*i)->_i->setPressure(pres); - } + case VNG2E: { + double tow; + uint16_t week; + uint8_t fix, numSats; + vec3d position; + vec3f ecefVel, ecefAcc; + float speedAcc, timeAcc; + + p.parseVNGPE(&tow, &week, &fix, &numSats, &position, &ecefVel, &ecefAcc, &speedAcc, &timeAcc); + + for (cditer i = o.begin(); i != o.end(); ++i) { + (*i)->_i->setGps2Tow(tow); + (*i)->_i->setGps2Week(week); + (*i)->_i->setFix2(static_cast(fix)); + (*i)->_i->setNumSats2(numSats); + (*i)->_i->setPositionGps2Ecef(position); + (*i)->_i->setVelocityGps2Ecef(ecefVel); + (*i)->_i->setPositionUncertaintyGps2Ecef(ecefAcc); + (*i)->_i->setVelocityUncertaintyGps2(speedAcc); + // Convert to uint32_t since this is the binary representation in nanoseconds. + (*i)->_i->setTimeUncertainty2(static_cast(timeAcc * 1e9)); + } + + break; + } - break; - } + case VNINS: { + double tow; + uint16_t week, status; + vec3d position; + vec3f ypr, nedVel; + float attUncertainty, velUncertainty, posUncertainty; + + p.parseVNINS( + &tow, &week, &status, &ypr, &position, &nedVel, &attUncertainty, &posUncertainty, + &velUncertainty); + + for (cditer i = o.begin(); i != o.end(); ++i) { + (*i)->_i->setTow(tow); + (*i)->_i->setWeek(week); + (*i)->_i->setInsStatus(static_cast(status)); + (*i)->_i->setYawPitchRoll(ypr); + (*i)->_i->setPositionEstimatedLla(position); + (*i)->_i->setVelocityEstimatedNed(nedVel); + // Binary data provides 3 components to yaw, pitch, roll uncertainty. + (*i)->_i->setAttitudeUncertainty(vec3f(attUncertainty)); + (*i)->_i->setPositionUncertaintyEstimated(posUncertainty); + (*i)->_i->setVelocityUncertaintyEstimated(velUncertainty); + } + + break; + } - case VNGPS: - { - double time; - uint16_t week; - uint8_t fix, numSats; - vec3d lla; - vec3f nedVel, nedAcc; - float speedAcc, timeAcc; - - p.parseVNGPS(&time, &week, &fix, &numSats, &lla, &nedVel, &nedAcc, &speedAcc, &timeAcc); - - for(cditer i = o.begin(); i != o.end(); ++i) { - (*i)->_i->setGpsTow(time); - (*i)->_i->setGpsWeek(week); - (*i)->_i->setFix(static_cast(fix)); - (*i)->_i->setNumSats(numSats); - (*i)->_i->setPositionGpsLla(lla); - (*i)->_i->setVelocityGpsNed(nedVel); - (*i)->_i->setPositionUncertaintyGpsNed(nedAcc); - (*i)->_i->setVelocityUncertaintyGps(speedAcc); - // Convert to uint32_t since this is the binary representation in nanoseconds. - (*i)->_i->setTimeUncertainty(static_cast(timeAcc * 1e9)); + case VNINE: { + double tow; + uint16_t week, status; + vec3d position; + vec3f ypr, velocity; + float attUncertainty, velUncertainty, posUncertainty; + + p.parseVNINE( + &tow, &week, &status, &ypr, &position, &velocity, &attUncertainty, &posUncertainty, + &velUncertainty); + + for (cditer i = o.begin(); i != o.end(); ++i) { + (*i)->_i->setTow(tow); + (*i)->_i->setWeek(week); + (*i)->_i->setInsStatus(static_cast(status)); + (*i)->_i->setYawPitchRoll(ypr); + (*i)->_i->setPositionEstimatedEcef(position); + (*i)->_i->setVelocityEstimatedEcef(velocity); + // Binary data provides 3 components to yaw, pitch, roll uncertainty. + (*i)->_i->setAttitudeUncertainty(vec3f(attUncertainty)); + (*i)->_i->setPositionUncertaintyEstimated(posUncertainty); + (*i)->_i->setVelocityUncertaintyEstimated(velUncertainty); + } + + break; } - break; - } + case VNISL: { + vec3f ypr, velocity, accel, ar; + vec3d lla; - case VNG2S: - { - double time; - uint16_t week; - uint8_t fix, numSats; - vec3d lla; - vec3f nedVel, nedAcc; - float speedAcc, timeAcc; + p.parseVNISL(&ypr, &lla, &velocity, &accel, &ar); - p.parseVNGPS(&time, &week, &fix, &numSats, &lla, &nedVel, &nedAcc, &speedAcc, &timeAcc); + for (cditer i = o.begin(); i != o.end(); ++i) { + (*i)->_i->setYawPitchRoll(ypr); + (*i)->_i->setPositionEstimatedLla(lla); + (*i)->_i->setVelocityEstimatedNed(velocity); + (*i)->_i->setAcceleration(accel); + (*i)->_i->setAngularRate(ar); + } - for(cditer i = o.begin(); i != o.end(); ++i) { - (*i)->_i->setGps2Tow(time); - (*i)->_i->setGps2Week(week); - (*i)->_i->setFix2(static_cast(fix)); - (*i)->_i->setNumSats2(numSats); - (*i)->_i->setPositionGps2Lla(lla); - (*i)->_i->setVelocityGps2Ned(nedVel); - (*i)->_i->setPositionUncertaintyGps2Ned(nedAcc); - (*i)->_i->setVelocityUncertaintyGps2(speedAcc); - // Convert to uint32_t since this is the binary representation in nanoseconds. - (*i)->_i->setTimeUncertainty2(static_cast(timeAcc * 1e9)); + break; } - break; - } + case VNISE: { + vec3f ypr, velocity, accel, ar; + vec3d position; - case VNGPE: - { - double tow; - uint16_t week; - uint8_t fix, numSats; - vec3d position; - vec3f ecefVel, ecefAcc; - float speedAcc, timeAcc; + p.parseVNISE(&ypr, &position, &velocity, &accel, &ar); - p.parseVNGPE(&tow, &week, &fix, &numSats, &position, &ecefVel, &ecefAcc, &speedAcc, &timeAcc); + for (cditer i = o.begin(); i != o.end(); ++i) { + (*i)->_i->setYawPitchRoll(ypr); + (*i)->_i->setPositionEstimatedEcef(position); + (*i)->_i->setVelocityEstimatedEcef(velocity); + (*i)->_i->setAcceleration(accel); + (*i)->_i->setAngularRate(ar); + } - for(cditer i = o.begin(); i != o.end(); ++i) { - (*i)->_i->setGpsTow(tow); - (*i)->_i->setGpsWeek(week); - (*i)->_i->setFix(static_cast(fix)); - (*i)->_i->setNumSats(numSats); - (*i)->_i->setPositionGpsEcef(position); - (*i)->_i->setVelocityGpsEcef(ecefVel); - (*i)->_i->setPositionUncertaintyGpsEcef(ecefAcc); - (*i)->_i->setVelocityUncertaintyGps(speedAcc); - // Convert to uint32_t since this is the binary representation in nanoseconds. - (*i)->_i->setTimeUncertainty(static_cast(timeAcc * 1e9)); + break; } - break; - } - - case VNG2E: - { - double tow; - uint16_t week; - uint8_t fix, numSats; - vec3d position; - vec3f ecefVel, ecefAcc; - float speedAcc, timeAcc; - - p.parseVNGPE(&tow, &week, &fix, &numSats, &position, &ecefVel, &ecefAcc, &speedAcc, &timeAcc); - - for(cditer i = o.begin(); i != o.end(); ++i) { - (*i)->_i->setGps2Tow(tow); - (*i)->_i->setGps2Week(week); - (*i)->_i->setFix2(static_cast(fix)); - (*i)->_i->setNumSats2(numSats); - (*i)->_i->setPositionGps2Ecef(position); - (*i)->_i->setVelocityGps2Ecef(ecefVel); - (*i)->_i->setPositionUncertaintyGps2Ecef(ecefAcc); - (*i)->_i->setVelocityUncertaintyGps2(speedAcc); - // Convert to uint32_t since this is the binary representation in nanoseconds. - (*i)->_i->setTimeUncertainty2(static_cast(timeAcc * 1e9)); + case VNDTV: { + float deltaTime; + vec3f deltaTheta, deltaVel; + + p.parseVNDTV(&deltaTime, &deltaTheta, &deltaVel); + + for (cditer i = o.begin(); i != o.end(); ++i) { + (*i)->_i->setDeltaTime(deltaTime); + (*i)->_i->setDeltaTheta(deltaTheta); + (*i)->_i->setDeltaVelocity(deltaVel); + } + + break; } - break; - } - - case VNINS: - { - double tow; - uint16_t week, status; - vec3d position; - vec3f ypr, nedVel; - float attUncertainty, velUncertainty, posUncertainty; - - p.parseVNINS(&tow, &week, &status, &ypr, &position, &nedVel, &attUncertainty, &posUncertainty, &velUncertainty); - - for (cditer i = o.begin(); i != o.end(); ++i) - { - (*i)->_i->setTow(tow); - (*i)->_i->setWeek(week); - (*i)->_i->setInsStatus(static_cast(status)); - (*i)->_i->setYawPitchRoll(ypr); - (*i)->_i->setPositionEstimatedLla(position); - (*i)->_i->setVelocityEstimatedNed(nedVel); - // Binary data provides 3 components to yaw, pitch, roll uncertainty. - (*i)->_i->setAttitudeUncertainty(vec3f(attUncertainty)); - (*i)->_i->setPositionUncertaintyEstimated(posUncertainty); - (*i)->_i->setVelocityUncertaintyEstimated(velUncertainty); - } - - break; - } - - case VNINE: - { - double tow; - uint16_t week, status; - vec3d position; - vec3f ypr, velocity; - float attUncertainty, velUncertainty, posUncertainty; - - p.parseVNINE(&tow, &week, &status, &ypr, &position, &velocity, &attUncertainty, &posUncertainty, &velUncertainty); - - for (cditer i = o.begin(); i != o.end(); ++i) - { - (*i)->_i->setTow(tow); - (*i)->_i->setWeek(week); - (*i)->_i->setInsStatus(static_cast(status)); - (*i)->_i->setYawPitchRoll(ypr); - (*i)->_i->setPositionEstimatedEcef(position); - (*i)->_i->setVelocityEstimatedEcef(velocity); - // Binary data provides 3 components to yaw, pitch, roll uncertainty. - (*i)->_i->setAttitudeUncertainty(vec3f(attUncertainty)); - (*i)->_i->setPositionUncertaintyEstimated(posUncertainty); - (*i)->_i->setVelocityUncertaintyEstimated(velUncertainty); - } - - break; - } - - case VNISL: - { - vec3f ypr, velocity, accel, ar; - vec3d lla; - - p.parseVNISL(&ypr, &lla, &velocity, &accel, &ar); - - for (cditer i = o.begin(); i != o.end(); ++i) - { - (*i)->_i->setYawPitchRoll(ypr); - (*i)->_i->setPositionEstimatedLla(lla); - (*i)->_i->setVelocityEstimatedNed(velocity); - (*i)->_i->setAcceleration(accel); - (*i)->_i->setAngularRate(ar); - } - - break; - } - - case VNISE: - { - vec3f ypr, velocity, accel, ar; - vec3d position; - - p.parseVNISE(&ypr, &position, &velocity, &accel, &ar); - - for (cditer i = o.begin(); i != o.end(); ++i) - { - (*i)->_i->setYawPitchRoll(ypr); - (*i)->_i->setPositionEstimatedEcef(position); - (*i)->_i->setVelocityEstimatedEcef(velocity); - (*i)->_i->setAcceleration(accel); - (*i)->_i->setAngularRate(ar); - } - - break; - } - - case VNDTV: - { - float deltaTime; - vec3f deltaTheta, deltaVel; - - p.parseVNDTV(&deltaTime, &deltaTheta, &deltaVel); - - for (cditer i = o.begin(); i != o.end(); ++i) - { - (*i)->_i->setDeltaTime(deltaTime); - (*i)->_i->setDeltaTheta(deltaTheta); - (*i)->_i->setDeltaVelocity(deltaVel); - } - - break; - } - - - default: - throw not_supported(); - - } -} - -void CompositeData::parseBinary(Packet& p, vector& o) -{ - BinaryGroup groups = static_cast(p.groups()); - size_t curGroupFieldIndex = 0; - - if (groups & BINARYGROUP_COMMON) - parseBinaryPacketCommonGroup(p, CommonGroup(p.groupField(curGroupFieldIndex++)), o); - if (groups & BINARYGROUP_TIME) - parseBinaryPacketTimeGroup(p, TimeGroup(p.groupField(curGroupFieldIndex++)), o); - if (groups & BINARYGROUP_IMU) - parseBinaryPacketImuGroup(p, ImuGroup(p.groupField(curGroupFieldIndex++)), o); - if (groups & BINARYGROUP_GPS) - parseBinaryPacketGpsGroup(p, GpsGroup(p.groupField(curGroupFieldIndex++)), o); - if (groups & BINARYGROUP_ATTITUDE) - parseBinaryPacketAttitudeGroup(p, AttitudeGroup(p.groupField(curGroupFieldIndex++)), o); - if (groups & BINARYGROUP_INS) - parseBinaryPacketInsGroup(p, InsGroup(p.groupField(curGroupFieldIndex++)), o); - if(groups & BINARYGROUP_GPS2) + default: + throw not_supported(); + } +} + +void CompositeData::parseBinary(Packet & p, vector & o) +{ + BinaryGroup groups = static_cast(p.groups()); + size_t curGroupFieldIndex = 0; + + if (groups & BINARYGROUP_COMMON) + parseBinaryPacketCommonGroup(p, CommonGroup(p.groupField(curGroupFieldIndex++)), o); + if (groups & BINARYGROUP_TIME) + parseBinaryPacketTimeGroup(p, TimeGroup(p.groupField(curGroupFieldIndex++)), o); + if (groups & BINARYGROUP_IMU) + parseBinaryPacketImuGroup(p, ImuGroup(p.groupField(curGroupFieldIndex++)), o); + if (groups & BINARYGROUP_GPS) + parseBinaryPacketGpsGroup(p, GpsGroup(p.groupField(curGroupFieldIndex++)), o); + if (groups & BINARYGROUP_ATTITUDE) + parseBinaryPacketAttitudeGroup(p, AttitudeGroup(p.groupField(curGroupFieldIndex++)), o); + if (groups & BINARYGROUP_INS) + parseBinaryPacketInsGroup(p, InsGroup(p.groupField(curGroupFieldIndex++)), o); + if (groups & BINARYGROUP_GPS2) parseBinaryPacketGps2Group(p, GpsGroup(p.groupField(curGroupFieldIndex++)), o); } -void CompositeData::parseBinaryPacketCommonGroup(Packet& p, CommonGroup gf, vector& o) +void CompositeData::parseBinaryPacketCommonGroup( + Packet & p, CommonGroup gf, vector & o) { - if (gf & COMMONGROUP_TIMESTARTUP) - setValues(p.extractUint64(), o, &Impl::setTimeStartup); + if (gf & COMMONGROUP_TIMESTARTUP) setValues(p.extractUint64(), o, &Impl::setTimeStartup); - if (gf & COMMONGROUP_TIMEGPS) - setValues(p.extractUint64(), o, &Impl::setTimeGps); + if (gf & COMMONGROUP_TIMEGPS) setValues(p.extractUint64(), o, &Impl::setTimeGps); - if (gf & COMMONGROUP_TIMESYNCIN) - setValues(p.extractUint64(), o, &Impl::setTimeSyncIn); + if (gf & COMMONGROUP_TIMESYNCIN) setValues(p.extractUint64(), o, &Impl::setTimeSyncIn); - if (gf & COMMONGROUP_YAWPITCHROLL) - setValues(p.extractVec3f(), o, &Impl::setYawPitchRoll); + if (gf & COMMONGROUP_YAWPITCHROLL) setValues(p.extractVec3f(), o, &Impl::setYawPitchRoll); - if (gf & COMMONGROUP_QUATERNION) - setValues(p.extractVec4f(), o, &Impl::setQuaternion); + if (gf & COMMONGROUP_QUATERNION) setValues(p.extractVec4f(), o, &Impl::setQuaternion); - if (gf & COMMONGROUP_ANGULARRATE) - setValues(p.extractVec3f(), o, &Impl::setAngularRate); + if (gf & COMMONGROUP_ANGULARRATE) setValues(p.extractVec3f(), o, &Impl::setAngularRate); - if (gf & COMMONGROUP_POSITION) - setValues(p.extractVec3d(), o, &Impl::setPositionEstimatedLla); + if (gf & COMMONGROUP_POSITION) setValues(p.extractVec3d(), o, &Impl::setPositionEstimatedLla); - if (gf & COMMONGROUP_VELOCITY) - setValues(p.extractVec3f(), o, &Impl::setVelocityEstimatedNed); + if (gf & COMMONGROUP_VELOCITY) setValues(p.extractVec3f(), o, &Impl::setVelocityEstimatedNed); - if (gf & COMMONGROUP_ACCEL) - setValues(p.extractVec3f(), o, &Impl::setAcceleration); + if (gf & COMMONGROUP_ACCEL) setValues(p.extractVec3f(), o, &Impl::setAcceleration); - if (gf & COMMONGROUP_IMU) - { - setValues(p.extractVec3f(), o, &Impl::setAccelerationUncompensated); - setValues(p.extractVec3f(), o, &Impl::setAngularRateUncompensated); - } - - if (gf & COMMONGROUP_MAGPRES) - { - setValues(p.extractVec3f(), o, &Impl::setMagnetic); - setValues(p.extractFloat(), o, &Impl::setTemperature); - setValues(p.extractFloat(), o, &Impl::setPressure); - } + if (gf & COMMONGROUP_IMU) { + setValues(p.extractVec3f(), o, &Impl::setAccelerationUncompensated); + setValues(p.extractVec3f(), o, &Impl::setAngularRateUncompensated); + } - if (gf & COMMONGROUP_DELTATHETA) - { - setValues(p.extractFloat(), o, &Impl::setDeltaTime); - setValues(p.extractVec3f(), o, &Impl::setDeltaTheta); - setValues(p.extractVec3f(), o, &Impl::setDeltaVelocity); - } + if (gf & COMMONGROUP_MAGPRES) { + setValues(p.extractVec3f(), o, &Impl::setMagnetic); + setValues(p.extractFloat(), o, &Impl::setTemperature); + setValues(p.extractFloat(), o, &Impl::setPressure); + } - if (gf & COMMONGROUP_INSSTATUS) - { - // Don't know if this is a VN-100, VN-200 or VN-300 so we can't know for sure if - // this is VpeStatus or InsStatus. - uint16_t v = p.extractUint16(); - setValues(VpeStatus(v), o, &Impl::setVpeStatus); - setValues(InsStatus(v), o, &Impl::setInsStatus); - } + if (gf & COMMONGROUP_DELTATHETA) { + setValues(p.extractFloat(), o, &Impl::setDeltaTime); + setValues(p.extractVec3f(), o, &Impl::setDeltaTheta); + setValues(p.extractVec3f(), o, &Impl::setDeltaVelocity); + } - if (gf & COMMONGROUP_SYNCINCNT) - setValues(p.extractUint32(), o, &Impl::setSyncInCnt); + if (gf & COMMONGROUP_INSSTATUS) { + // Don't know if this is a VN-100, VN-200 or VN-300 so we can't know for sure if + // this is VpeStatus or InsStatus. + uint16_t v = p.extractUint16(); + setValues(VpeStatus(v), o, &Impl::setVpeStatus); + setValues(InsStatus(v), o, &Impl::setInsStatus); + } - if (gf & COMMONGROUP_TIMEGPSPPS) - setValues(p.extractUint64(), o, &Impl::setTimeGpsPps); + if (gf & COMMONGROUP_SYNCINCNT) setValues(p.extractUint32(), o, &Impl::setSyncInCnt); + if (gf & COMMONGROUP_TIMEGPSPPS) setValues(p.extractUint64(), o, &Impl::setTimeGpsPps); } -void CompositeData::parseBinaryPacketTimeGroup(Packet& p, TimeGroup gf, vector& o) +void CompositeData::parseBinaryPacketTimeGroup( + Packet & p, TimeGroup gf, vector & o) { - if (gf & TIMEGROUP_TIMESTARTUP) - setValues(p.extractUint64(), o, &Impl::setTimeStartup); + if (gf & TIMEGROUP_TIMESTARTUP) setValues(p.extractUint64(), o, &Impl::setTimeStartup); - if (gf & TIMEGROUP_TIMEGPS) - setValues(p.extractUint64(), o, &Impl::setTimeGps); + if (gf & TIMEGROUP_TIMEGPS) setValues(p.extractUint64(), o, &Impl::setTimeGps); - if (gf & TIMEGROUP_GPSTOW) - setValues(((double)p.extractUint64()/1000000000), o, &Impl::setTow); + if (gf & TIMEGROUP_GPSTOW) setValues(((double)p.extractUint64() / 1000000000), o, &Impl::setTow); - if (gf & TIMEGROUP_GPSWEEK) - setValues(p.extractUint16(), o, &Impl::setWeek); + if (gf & TIMEGROUP_GPSWEEK) setValues(p.extractUint16(), o, &Impl::setWeek); - if (gf & TIMEGROUP_TIMESYNCIN) - setValues(p.extractUint64(), o, &Impl::setTimeSyncIn); + if (gf & TIMEGROUP_TIMESYNCIN) setValues(p.extractUint64(), o, &Impl::setTimeSyncIn); - if (gf & TIMEGROUP_TIMEGPSPPS) - setValues(p.extractUint64(), o, &Impl::setTimeGpsPps); + if (gf & TIMEGROUP_TIMEGPSPPS) setValues(p.extractUint64(), o, &Impl::setTimeGpsPps); - if (gf & TIMEGROUP_TIMEUTC) - { - TimeUtc t; + if (gf & TIMEGROUP_TIMEUTC) { + TimeUtc t; - t.year = p.extractInt8(); - t.month = p.extractUint8(); - t.day = p.extractUint8(); - t.hour = p.extractUint8(); - t.min = p.extractUint8(); - t.sec = p.extractUint8(); - t.ms = p.extractUint16(); + t.year = p.extractInt8(); + t.month = p.extractUint8(); + t.day = p.extractUint8(); + t.hour = p.extractUint8(); + t.min = p.extractUint8(); + t.sec = p.extractUint8(); + t.ms = p.extractUint16(); - setValues(t, o, &Impl::setTimeUtc); - } + setValues(t, o, &Impl::setTimeUtc); + } - if (gf & TIMEGROUP_SYNCINCNT) - setValues(p.extractUint32(), o, &Impl::setSyncInCnt); + if (gf & TIMEGROUP_SYNCINCNT) setValues(p.extractUint32(), o, &Impl::setSyncInCnt); - if (gf & TIMEGROUP_SYNCOUTCNT) - setValues(p.extractUint32(), o, &Impl::setSyncOutCnt); + if (gf & TIMEGROUP_SYNCOUTCNT) setValues(p.extractUint32(), o, &Impl::setSyncOutCnt); - if (gf & TIMEGROUP_TIMESTATUS) - setValues(p.extractUint8(), o, &Impl::setTimeStatus); + if (gf & TIMEGROUP_TIMESTATUS) setValues(p.extractUint8(), o, &Impl::setTimeStatus); } -void CompositeData::parseBinaryPacketImuGroup(Packet& p, ImuGroup gf, vector& o) +void CompositeData::parseBinaryPacketImuGroup(Packet & p, ImuGroup gf, vector & o) { - if (gf & IMUGROUP_IMUSTATUS) - // This field is currently reserved. - p.extractUint16(); - - if (gf & IMUGROUP_UNCOMPMAG) - setValues(p.extractVec3f(), o, &Impl::setMagneticUncompensated); + if (gf & IMUGROUP_IMUSTATUS) + // This field is currently reserved. + p.extractUint16(); - if (gf & IMUGROUP_UNCOMPACCEL) - setValues(p.extractVec3f(), o, &Impl::setAccelerationUncompensated); + if (gf & IMUGROUP_UNCOMPMAG) setValues(p.extractVec3f(), o, &Impl::setMagneticUncompensated); - if (gf & IMUGROUP_UNCOMPGYRO) - setValues(p.extractVec3f(), o, &Impl::setAngularRateUncompensated); + if (gf & IMUGROUP_UNCOMPACCEL) + setValues(p.extractVec3f(), o, &Impl::setAccelerationUncompensated); - if (gf & IMUGROUP_TEMP) - setValues(p.extractFloat(), o, &Impl::setTemperature); + if (gf & IMUGROUP_UNCOMPGYRO) setValues(p.extractVec3f(), o, &Impl::setAngularRateUncompensated); - if (gf & IMUGROUP_PRES) - setValues(p.extractFloat(), o, &Impl::setPressure); + if (gf & IMUGROUP_TEMP) setValues(p.extractFloat(), o, &Impl::setTemperature); - if (gf & IMUGROUP_DELTATHETA) - { - setValues(p.extractFloat(), o, &Impl::setDeltaTime); - setValues(p.extractVec3f(), o, &Impl::setDeltaTheta); - } + if (gf & IMUGROUP_PRES) setValues(p.extractFloat(), o, &Impl::setPressure); - if (gf & IMUGROUP_DELTAVEL) - setValues(p.extractVec3f(), o, &Impl::setDeltaVelocity); + if (gf & IMUGROUP_DELTATHETA) { + setValues(p.extractFloat(), o, &Impl::setDeltaTime); + setValues(p.extractVec3f(), o, &Impl::setDeltaTheta); + } - if (gf & IMUGROUP_MAG) - setValues(p.extractVec3f(), o, &Impl::setMagnetic); + if (gf & IMUGROUP_DELTAVEL) setValues(p.extractVec3f(), o, &Impl::setDeltaVelocity); - if (gf & IMUGROUP_ACCEL) - setValues(p.extractVec3f(), o, &Impl::setAcceleration); + if (gf & IMUGROUP_MAG) setValues(p.extractVec3f(), o, &Impl::setMagnetic); - if (gf & IMUGROUP_ANGULARRATE) - setValues(p.extractVec3f(), o, &Impl::setAngularRate); + if (gf & IMUGROUP_ACCEL) setValues(p.extractVec3f(), o, &Impl::setAcceleration); - if (gf & IMUGROUP_SENSSAT) - setValues(SensSat(p.extractUint16()), o, &Impl::setSensSat); + if (gf & IMUGROUP_ANGULARRATE) setValues(p.extractVec3f(), o, &Impl::setAngularRate); + if (gf & IMUGROUP_SENSSAT) setValues(SensSat(p.extractUint16()), o, &Impl::setSensSat); } -void CompositeData::parseBinaryPacketGpsGroup(Packet& p, GpsGroup gf, vector& o) +void CompositeData::parseBinaryPacketGpsGroup(Packet & p, GpsGroup gf, vector & o) { - if (gf & GPSGROUP_UTC) - { - TimeUtc t; + if (gf & GPSGROUP_UTC) { + TimeUtc t; - t.year = p.extractInt8(); - t.month = p.extractUint8(); - t.day = p.extractUint8(); - t.hour = p.extractUint8(); - t.min = p.extractUint8(); - t.sec = p.extractUint8(); - t.ms = p.extractUint16(); + t.year = p.extractInt8(); + t.month = p.extractUint8(); + t.day = p.extractUint8(); + t.hour = p.extractUint8(); + t.min = p.extractUint8(); + t.sec = p.extractUint8(); + t.ms = p.extractUint16(); - setValues(t, o, &Impl::setTimeUtc); - } + setValues(t, o, &Impl::setTimeUtc); + } - if (gf & GPSGROUP_TOW) - setValues(p.extractUint64(), o, &Impl::setGpsTow); + if (gf & GPSGROUP_TOW) setValues(p.extractUint64(), o, &Impl::setGpsTow); - if (gf & GPSGROUP_WEEK) - setValues(p.extractUint16(), o, &Impl::setGpsWeek); + if (gf & GPSGROUP_WEEK) setValues(p.extractUint16(), o, &Impl::setGpsWeek); - if (gf & GPSGROUP_NUMSATS) - setValues(p.extractUint8(), o, &Impl::setNumSats); + if (gf & GPSGROUP_NUMSATS) setValues(p.extractUint8(), o, &Impl::setNumSats); - if (gf & GPSGROUP_FIX) - setValues(GpsFix(p.extractUint8()), o, &Impl::setFix); + if (gf & GPSGROUP_FIX) setValues(GpsFix(p.extractUint8()), o, &Impl::setFix); - if (gf & GPSGROUP_POSLLA) - setValues(p.extractVec3d(), o, &Impl::setPositionGpsLla); + if (gf & GPSGROUP_POSLLA) setValues(p.extractVec3d(), o, &Impl::setPositionGpsLla); - if (gf & GPSGROUP_POSECEF) - setValues(p.extractVec3d(), o, &Impl::setPositionGpsEcef); + if (gf & GPSGROUP_POSECEF) setValues(p.extractVec3d(), o, &Impl::setPositionGpsEcef); - if (gf & GPSGROUP_VELNED) - setValues(p.extractVec3f(), o, &Impl::setVelocityGpsNed); + if (gf & GPSGROUP_VELNED) setValues(p.extractVec3f(), o, &Impl::setVelocityGpsNed); - if (gf & GPSGROUP_VELECEF) - setValues(p.extractVec3f(), o, &Impl::setVelocityGpsEcef); + if (gf & GPSGROUP_VELECEF) setValues(p.extractVec3f(), o, &Impl::setVelocityGpsEcef); - if (gf & GPSGROUP_POSU) - setValues(p.extractVec3f(), o, &Impl::setPositionUncertaintyGpsNed); + if (gf & GPSGROUP_POSU) setValues(p.extractVec3f(), o, &Impl::setPositionUncertaintyGpsNed); - if (gf & GPSGROUP_VELU) - setValues(p.extractFloat(), o, &Impl::setVelocityUncertaintyGps); + if (gf & GPSGROUP_VELU) setValues(p.extractFloat(), o, &Impl::setVelocityUncertaintyGps); - if (gf & GPSGROUP_TIMEU) - setValues(p.extractUint32(), o, &Impl::setTimeUncertainty); + if (gf & GPSGROUP_TIMEU) setValues(p.extractUint32(), o, &Impl::setTimeUncertainty); - if (gf & GPSGROUP_TIMEINFO) - { - TimeInfo t; + if (gf & GPSGROUP_TIMEINFO) { + TimeInfo t; - t.timeStatus = p.extractUint8(); - t.leapSecs = p.extractInt8(); + t.timeStatus = p.extractUint8(); + t.leapSecs = p.extractInt8(); - setValues(t, o, &Impl::setTimeInfo); + setValues(t, o, &Impl::setTimeInfo); } - if (gf & GPSGROUP_DOP) - { + if (gf & GPSGROUP_DOP) { GnssDop d; d.gDop = p.extractFloat(); @@ -2759,77 +2384,59 @@ void CompositeData::parseBinaryPacketGpsGroup(Packet& p, GpsGroup gf, vector& o) +void CompositeData::parseBinaryPacketAttitudeGroup( + Packet & p, AttitudeGroup gf, vector & o) { - if (gf & ATTITUDEGROUP_VPESTATUS) - setValues(VpeStatus(p.extractUint16()), o, &Impl::setVpeStatus); + if (gf & ATTITUDEGROUP_VPESTATUS) setValues(VpeStatus(p.extractUint16()), o, &Impl::setVpeStatus); - if (gf & ATTITUDEGROUP_YAWPITCHROLL) - setValues(p.extractVec3f(), o, &Impl::setYawPitchRoll); + if (gf & ATTITUDEGROUP_YAWPITCHROLL) setValues(p.extractVec3f(), o, &Impl::setYawPitchRoll); - if (gf & ATTITUDEGROUP_QUATERNION) - setValues(p.extractVec4f(), o, &Impl::setQuaternion); + if (gf & ATTITUDEGROUP_QUATERNION) setValues(p.extractVec4f(), o, &Impl::setQuaternion); - if (gf & ATTITUDEGROUP_DCM) - setValues(p.extractMat3f(), o, &Impl::setDirectionConsineMatrix); + if (gf & ATTITUDEGROUP_DCM) setValues(p.extractMat3f(), o, &Impl::setDirectionConsineMatrix); - if (gf & ATTITUDEGROUP_MAGNED) - setValues(p.extractVec3f(), o, &Impl::setMagneticNed); + if (gf & ATTITUDEGROUP_MAGNED) setValues(p.extractVec3f(), o, &Impl::setMagneticNed); - if (gf & ATTITUDEGROUP_ACCELNED) - setValues(p.extractVec3f(), o, &Impl::setAccelerationNed); + if (gf & ATTITUDEGROUP_ACCELNED) setValues(p.extractVec3f(), o, &Impl::setAccelerationNed); - if (gf & ATTITUDEGROUP_LINEARACCELBODY) - setValues(p.extractVec3f(), o, &Impl::setAccelerationLinearBody); + if (gf & ATTITUDEGROUP_LINEARACCELBODY) + setValues(p.extractVec3f(), o, &Impl::setAccelerationLinearBody); - if (gf & ATTITUDEGROUP_LINEARACCELNED) - setValues(p.extractVec3f(), o, &Impl::setAccelerationLinearNed); - - if (gf & ATTITUDEGROUP_YPRU) - setValues(p.extractVec3f(), o, &Impl::setAttitudeUncertainty); + if (gf & ATTITUDEGROUP_LINEARACCELNED) + setValues(p.extractVec3f(), o, &Impl::setAccelerationLinearNed); + if (gf & ATTITUDEGROUP_YPRU) setValues(p.extractVec3f(), o, &Impl::setAttitudeUncertainty); } -void CompositeData::parseBinaryPacketInsGroup(Packet& p, InsGroup gf, vector& o) +void CompositeData::parseBinaryPacketInsGroup(Packet & p, InsGroup gf, vector & o) { - if (gf & INSGROUP_INSSTATUS) - setValues(InsStatus(p.extractUint16()), o, &Impl::setInsStatus); - - if (gf & INSGROUP_POSLLA) - setValues(p.extractVec3d(), o, &Impl::setPositionEstimatedLla); + if (gf & INSGROUP_INSSTATUS) setValues(InsStatus(p.extractUint16()), o, &Impl::setInsStatus); - if (gf & INSGROUP_POSECEF) - setValues(p.extractVec3d(), o, &Impl::setPositionEstimatedEcef); + if (gf & INSGROUP_POSLLA) setValues(p.extractVec3d(), o, &Impl::setPositionEstimatedLla); - if (gf & INSGROUP_VELBODY) - setValues(p.extractVec3f(), o, &Impl::setVelocityEstimatedBody); + if (gf & INSGROUP_POSECEF) setValues(p.extractVec3d(), o, &Impl::setPositionEstimatedEcef); - if (gf & INSGROUP_VELNED) - setValues(p.extractVec3f(), o, &Impl::setVelocityEstimatedNed); + if (gf & INSGROUP_VELBODY) setValues(p.extractVec3f(), o, &Impl::setVelocityEstimatedBody); - if (gf & INSGROUP_VELECEF) - setValues(p.extractVec3f(), o, &Impl::setVelocityEstimatedEcef); + if (gf & INSGROUP_VELNED) setValues(p.extractVec3f(), o, &Impl::setVelocityEstimatedNed); - if (gf & INSGROUP_MAGECEF) - setValues(p.extractVec3f(), o, &Impl::setMagneticEcef); + if (gf & INSGROUP_VELECEF) setValues(p.extractVec3f(), o, &Impl::setVelocityEstimatedEcef); - if (gf & INSGROUP_ACCELECEF) - setValues(p.extractVec3f(), o, &Impl::setAccelerationEcef); + if (gf & INSGROUP_MAGECEF) setValues(p.extractVec3f(), o, &Impl::setMagneticEcef); - if (gf & INSGROUP_LINEARACCELECEF) - setValues(p.extractVec3f(), o, &Impl::setAccelerationLinearEcef); + if (gf & INSGROUP_ACCELECEF) setValues(p.extractVec3f(), o, &Impl::setAccelerationEcef); - if (gf & INSGROUP_POSU) - setValues(p.extractFloat(), o, &Impl::setPositionUncertaintyEstimated); + if (gf & INSGROUP_LINEARACCELECEF) + setValues(p.extractVec3f(), o, &Impl::setAccelerationLinearEcef); - if (gf & INSGROUP_VELU) - setValues(p.extractFloat(), o, &Impl::setVelocityUncertaintyEstimated); + if (gf & INSGROUP_POSU) setValues(p.extractFloat(), o, &Impl::setPositionUncertaintyEstimated); + if (gf & INSGROUP_VELU) setValues(p.extractFloat(), o, &Impl::setVelocityUncertaintyEstimated); } -void CompositeData::parseBinaryPacketGps2Group(Packet& p, GpsGroup gf, vector& o) +void CompositeData::parseBinaryPacketGps2Group(Packet & p, GpsGroup gf, vector & o) { - if(gf & GPSGROUP_UTC) { + if (gf & GPSGROUP_UTC) { TimeUtc t; t.year = p.extractInt8(); @@ -2843,40 +2450,29 @@ void CompositeData::parseBinaryPacketGps2Group(Packet& p, GpsGroup gf, vector maxNum) - { - maxNum = b2[i]; - maxIndex = i; - } - } - - vec4f q; - - switch (maxIndex) - { - case 0: - q.w = sqrt(b2[0]); - q.x = (dcm.e12 - dcm.e21) / 4.f / q.w; - q.y = (dcm.e20 - dcm.e02) / 4.f / q.w; - q.z = (dcm.e01 - dcm.e10) / 4.f / q.w; - break; - - case 1: - q.x = sqrt(b2[1]); - q.w = (dcm.e12 - dcm.e21) / 4.f / q.x; - if (q.w < 0) - { - q.x = -q.x; - q.w = -q.w; - } - q.y = (dcm.e01 + dcm.e10) / 4.f / q.x; - q.z = (dcm.e20 + dcm.e02) / 4.f / q.x; - break; - - case 2: - q.y = sqrt(b2[2]); - q.w = (dcm.e20 - dcm.e02) / 4.f / q.y; - if (q.w < 0) - { - q.y = -q.y; - q.w = -q.w; - } - q.x = (dcm.e01 + dcm.e10) / 4.f / q.y; - q.z = (dcm.e12 + dcm.e21) / 4.f / q.y; - break; - - case 3: - q.z = sqrt(b2[3]); - q.w = (dcm.e01 - dcm.e10) / 4.f / q.z; - if (q.w < 0) - { - q.z = -q.z; - q.w = -q.w; - } - q.x = (dcm.e20 + dcm.e02) / 4.f / q.z; - q.y = (dcm.e12 + dcm.e21) / 4.f / q.z; - break; - } - - return q; -} - -float course_over_ground(float velNedX, float velNedY) -{ - return atan2(velNedY, velNedX); -} - -float course_over_ground(vec3f velNed) -{ - return course_over_ground(velNed.x, velNed.y); -} - -float speed_over_ground(float velNedX, float velNedY) -{ - return vec2f(velNedX, velNedY).mag(); -} - -float speed_over_ground(vec3f velNed) -{ - return speed_over_ground(velNed.x, velNed.y); -} + float tr = dcm.e00 + dcm.e11 + dcm.e22; + + float b2[] = { + (1.f + tr) / 4.f, (1.f + 2.f * dcm.e00 - tr) / 4.f, (1.f + 2.f * dcm.e11 - tr) / 4.f, + (1.f + 2.f * dcm.e22 - tr) / 4.f}; + + float maxNum = b2[0]; + size_t maxIndex = 0; + for (size_t i = 1; i < 4; i++) { + if (b2[i] > maxNum) { + maxNum = b2[i]; + maxIndex = i; + } + } + + vec4f q; + + switch (maxIndex) { + case 0: + q.w = sqrt(b2[0]); + q.x = (dcm.e12 - dcm.e21) / 4.f / q.w; + q.y = (dcm.e20 - dcm.e02) / 4.f / q.w; + q.z = (dcm.e01 - dcm.e10) / 4.f / q.w; + break; + + case 1: + q.x = sqrt(b2[1]); + q.w = (dcm.e12 - dcm.e21) / 4.f / q.x; + if (q.w < 0) { + q.x = -q.x; + q.w = -q.w; + } + q.y = (dcm.e01 + dcm.e10) / 4.f / q.x; + q.z = (dcm.e20 + dcm.e02) / 4.f / q.x; + break; + + case 2: + q.y = sqrt(b2[2]); + q.w = (dcm.e20 - dcm.e02) / 4.f / q.y; + if (q.w < 0) { + q.y = -q.y; + q.w = -q.w; + } + q.x = (dcm.e01 + dcm.e10) / 4.f / q.y; + q.z = (dcm.e12 + dcm.e21) / 4.f / q.y; + break; + + case 3: + q.z = sqrt(b2[3]); + q.w = (dcm.e01 - dcm.e10) / 4.f / q.z; + if (q.w < 0) { + q.z = -q.z; + q.w = -q.w; + } + q.x = (dcm.e20 + dcm.e02) / 4.f / q.z; + q.y = (dcm.e12 + dcm.e21) / 4.f / q.z; + break; + } + + return q; +} + +float course_over_ground(float velNedX, float velNedY) { return atan2(velNedY, velNedX); } + +float course_over_ground(vec3f velNed) { return course_over_ground(velNed.x, velNed.y); } + +float speed_over_ground(float velNedX, float velNedY) { return vec2f(velNedX, velNedY).mag(); } + +float speed_over_ground(vec3f velNed) { return speed_over_ground(velNed.x, velNed.y); } vec3f quat2omegaPhiKappaInRads(vec4f quat) { - float q1 = quat.x; - float q2 = quat.y; - float q3 = quat.z; - float q4 = quat.w; + float q1 = quat.x; + float q2 = quat.y; + float q3 = quat.z; + float q4 = quat.w; - float omega = atanf( -2*(q1*q3 - q3*q4) / (q3*q3 - q4*q4 - q1*q1 + q2*q2) ); - float phi = asinf( 2*(q4*q2 + q3*q1) ); - float kappa = atanf( -2*(q4*q1 - q3*q2) / (q3*q3 + q4*q4 - q1*q1 - q2*q2) ); + float omega = atanf(-2 * (q1 * q3 - q3 * q4) / (q3 * q3 - q4 * q4 - q1 * q1 + q2 * q2)); + float phi = asinf(2 * (q4 * q2 + q3 * q1)); + float kappa = atanf(-2 * (q4 * q1 - q3 * q2) / (q3 * q3 + q4 * q4 - q1 * q1 - q2 * q2)); - return vec3f(omega, phi, kappa); + return vec3f(omega, phi, kappa); } vec3f dcm2omegaPhiKappaInRads(mat3f dcm) { - vec4f quat = dcm2quat(dcm); + vec4f quat = dcm2quat(dcm); - return quat2omegaPhiKappaInRads(quat); + return quat2omegaPhiKappaInRads(quat); } vec3f yprInDegs2omegaPhiKappaInRads(vec3f yprDegs) { - vec4f quat = yprInDegs2Quat(yprDegs); + vec4f quat = yprInDegs2Quat(yprDegs); - return quat2omegaPhiKappaInRads(quat); + return quat2omegaPhiKappaInRads(quat); } vec3f yprInRads2omegaPhiKappaInRads(vec3f yprRads) { - vec4f quat = yprInRads2Quat(yprRads); + vec4f quat = yprInRads2Quat(yprRads); - return quat2omegaPhiKappaInRads(quat); + return quat2omegaPhiKappaInRads(quat); } -} -} +} // namespace math +} // namespace vn diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/criticalsection.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/criticalsection.cpp index 63688ef3..e5f49180 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/criticalsection.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/criticalsection.cpp @@ -1,81 +1,80 @@ #include "vn/criticalsection.h" #if _WIN32 - #include +#include #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - #include +#include #else - #error "Unknown System" +#error "Unknown System" #endif #include "vn/exceptions.h" using namespace std; -namespace vn { -namespace xplat { +namespace vn +{ +namespace xplat +{ struct CriticalSection::Impl { - #if _WIN32 - CRITICAL_SECTION CriticalSection; - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - pthread_mutex_t CriticalSection; - #else - #error "Unknown System" - #endif +#if _WIN32 + CRITICAL_SECTION CriticalSection; +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + pthread_mutex_t CriticalSection; +#else +#error "Unknown System" +#endif - Impl() - { - } + Impl() {} }; -CriticalSection::CriticalSection() : - _pi(new Impl()) +CriticalSection::CriticalSection() : _pi(new Impl()) { - #if _WIN32 - InitializeCriticalSection(&_pi->CriticalSection); - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - pthread_mutex_init(&_pi->CriticalSection, NULL); - #else - #error "Unknown System" - #endif +#if _WIN32 + InitializeCriticalSection(&_pi->CriticalSection); +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + pthread_mutex_init(&_pi->CriticalSection, NULL); +#else +#error "Unknown System" +#endif } CriticalSection::~CriticalSection() { - #if _WIN32 - DeleteCriticalSection(&_pi->CriticalSection); - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - pthread_mutex_destroy(&_pi->CriticalSection); - #else - #error "Unknown System" - #endif +#if _WIN32 + DeleteCriticalSection(&_pi->CriticalSection); +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + pthread_mutex_destroy(&_pi->CriticalSection); +#else +#error "Unknown System" +#endif - delete _pi; + delete _pi; } void CriticalSection::enter() { - #if _WIN32 - EnterCriticalSection(&_pi->CriticalSection); - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - pthread_mutex_lock(&_pi->CriticalSection); - #else - #error "Unknown System" - #endif +#if _WIN32 + EnterCriticalSection(&_pi->CriticalSection); +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + pthread_mutex_lock(&_pi->CriticalSection); +#else +#error "Unknown System" +#endif } void CriticalSection::leave() { - #if _WIN32 - LeaveCriticalSection(&_pi->CriticalSection); - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - pthread_mutex_unlock(&_pi->CriticalSection); - #else - #error "Unknown System" - #endif +#if _WIN32 + LeaveCriticalSection(&_pi->CriticalSection); +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + pthread_mutex_unlock(&_pi->CriticalSection); +#else +#error "Unknown System" +#endif } -} -} +} // namespace xplat +} // namespace vn diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/dllvalidator.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/dllvalidator.cpp index fe50dccb..5dd0e903 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/dllvalidator.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/dllvalidator.cpp @@ -4,120 +4,103 @@ #include -void dumpImportDirectory32(PeLib::PeFile32& pef, std::vector& dllNamesOut) +void dumpImportDirectory32(PeLib::PeFile32 & pef, std::vector & dllNamesOut) { - if (pef.readImportDirectory() == PeLib::ERROR_NO_ERROR) - { - const PeLib::ImportDirectory32& imp = static_cast(pef).impDir(); - - for (unsigned int i = 0; i < imp.getNumberOfFiles(PeLib::OLDDIR); i++) - { - dllNamesOut.push_back(imp.getFileName(i, PeLib::OLDDIR)); - } - } -} + if (pef.readImportDirectory() == PeLib::ERROR_NO_ERROR) { + const PeLib::ImportDirectory32 & imp = static_cast(pef).impDir(); -void dumpImportDirectory64(PeLib::PeFile64& pef, std::vector& dllNamesOut) -{ - if (pef.readImportDirectory() == PeLib::ERROR_NO_ERROR) - { - const PeLib::ImportDirectory64& imp = static_cast(pef).impDir(); - - for (unsigned int i = 0; i < imp.getNumberOfFiles(PeLib::OLDDIR); i++) - { - dllNamesOut.push_back(imp.getFileName(i, PeLib::OLDDIR)); - } - } + for (unsigned int i = 0; i < imp.getNumberOfFiles(PeLib::OLDDIR); i++) { + dllNamesOut.push_back(imp.getFileName(i, PeLib::OLDDIR)); + } + } } -void DllValidator::DllValidatorVisitor::callback(PeLib::PeFile32 &file) +void dumpImportDirectory64(PeLib::PeFile64 & pef, std::vector & dllNamesOut) { - dumpImportDirectory32(file, mRequiredDlls); + if (pef.readImportDirectory() == PeLib::ERROR_NO_ERROR) { + const PeLib::ImportDirectory64 & imp = static_cast(pef).impDir(); + + for (unsigned int i = 0; i < imp.getNumberOfFiles(PeLib::OLDDIR); i++) { + dllNamesOut.push_back(imp.getFileName(i, PeLib::OLDDIR)); + } + } } -void DllValidator::DllValidatorVisitor::callback(PeLib::PeFile64 &file) +void DllValidator::DllValidatorVisitor::callback(PeLib::PeFile32 & file) { - dumpImportDirectory64(file, mRequiredDlls); + dumpImportDirectory32(file, mRequiredDlls); } -DllValidator::DllValidator() +void DllValidator::DllValidatorVisitor::callback(PeLib::PeFile64 & file) { + dumpImportDirectory64(file, mRequiredDlls); } -DllValidator::DllValidator(std::string dllName, std::string currentDirectory) : - mIsInitialized(false), - mIsValid(false), - mPeFile(NULL), - mFileName(dllName), - mWorkingDirectory(currentDirectory) +DllValidator::DllValidator() {} + +DllValidator::DllValidator(std::string dllName, std::string currentDirectory) +: mIsInitialized(false), + mIsValid(false), + mPeFile(NULL), + mFileName(dllName), + mWorkingDirectory(currentDirectory) { } bool DllValidator::initialize() { - mPeFile = PeLib::openPeFile(mFileName); - - if (mPeFile != NULL) - { - if ((mPeFile->readMzHeader() == 0) && - (mPeFile->readPeHeader() == 0)) - { - mIsInitialized = true; - } - } - - return mIsInitialized; -} + mPeFile = PeLib::openPeFile(mFileName); -bool DllValidator::hasDllNames() -{ - return mVisitor.mRequiredDlls.size() > 0; + if (mPeFile != NULL) { + if ((mPeFile->readMzHeader() == 0) && (mPeFile->readPeHeader() == 0)) { + mIsInitialized = true; + } + } + + return mIsInitialized; } -void DllValidator::getDllNames(std::vector& dllNamesOut) +bool DllValidator::hasDllNames() { return mVisitor.mRequiredDlls.size() > 0; } + +void DllValidator::getDllNames(std::vector & dllNamesOut) { - dllNamesOut.clear(); - dllNamesOut = mVisitor.mRequiredDlls; + dllNamesOut.clear(); + dllNamesOut = mVisitor.mRequiredDlls; } -void DllValidator::getMissingDllNames(std::vector& missingDllNamesOut) +void DllValidator::getMissingDllNames(std::vector & missingDllNamesOut) { - missingDllNamesOut.clear(); - missingDllNamesOut = mMissingDlls; + missingDllNamesOut.clear(); + missingDllNamesOut = mMissingDlls; } bool DllValidator::validate() { - mIsValid = false; - - if (mIsInitialized) - { - SetCurrentDirectory(mWorkingDirectory.c_str()); - - mMissingDlls.clear(); - - mPeFile->visit(mVisitor); - - for (size_t index = 0; index < mVisitor.mRequiredDlls.size(); index++) - { - HMODULE module = LoadLibraryEx(mVisitor.mRequiredDlls[index].c_str(), NULL, LOAD_LIBRARY_AS_IMAGE_RESOURCE); - if (NULL == module) - { - mMissingDlls.push_back(mVisitor.mRequiredDlls[index]); - } - } - } - else if (mPeFile == NULL) - { - mMissingDlls.push_back(mFileName + " is not a valid DLL file."); - } - - if (mMissingDlls.empty()) - { - mIsValid = true; - } - - return mIsValid; + mIsValid = false; + + if (mIsInitialized) { + SetCurrentDirectory(mWorkingDirectory.c_str()); + + mMissingDlls.clear(); + + mPeFile->visit(mVisitor); + + for (size_t index = 0; index < mVisitor.mRequiredDlls.size(); index++) { + HMODULE module = + LoadLibraryEx(mVisitor.mRequiredDlls[index].c_str(), NULL, LOAD_LIBRARY_AS_IMAGE_RESOURCE); + if (NULL == module) { + mMissingDlls.push_back(mVisitor.mRequiredDlls[index]); + } + } + } else if (mPeFile == NULL) { + mMissingDlls.push_back(mFileName + " is not a valid DLL file."); + } + + if (mMissingDlls.empty()) { + mIsValid = true; + } + + return mIsValid; } #endif diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/error_detection.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/error_detection.cpp index 6f4e9636..639e269b 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/error_detection.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/error_detection.cpp @@ -3,43 +3,43 @@ #include using namespace std; -namespace vn { -namespace data { -namespace integrity { +namespace vn +{ +namespace data +{ +namespace integrity +{ uint8_t Checksum8::compute(char const data[], size_t length) { - if (length > 1000) - cout << "HAPPENING!" << endl; + if (length > 1000) cout << "HAPPENING!" << endl; - uint8_t xorVal = 0; + uint8_t xorVal = 0; - for (size_t i = 0; i < length; i++) - { - xorVal ^= data[i]; - } + for (size_t i = 0; i < length; i++) { + xorVal ^= data[i]; + } - return xorVal; + return xorVal; } uint16_t Crc16::compute(char const data[], size_t length) { - uint32_t i; - uint16_t crc = 0; + uint32_t i; + uint16_t crc = 0; - for (i = 0; i < length; i++) - { - crc = static_cast((crc >> 8) | (crc << 8)); + for (i = 0; i < length; i++) { + crc = static_cast((crc >> 8) | (crc << 8)); - crc ^= static_cast(data[i]); - crc ^= static_cast(static_cast(crc & 0xFF) >> 4); - crc ^= static_cast((crc << 8) << 4); - crc ^= static_cast(((crc & 0xFF) << 4) << 1); - } + crc ^= static_cast(data[i]); + crc ^= static_cast(static_cast(crc & 0xFF) >> 4); + crc ^= static_cast((crc << 8) << 4); + crc ^= static_cast(((crc & 0xFF) << 4) << 1); + } - return crc; + return crc; } -} -} -} +} // namespace integrity +} // namespace data +} // namespace vn diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/event.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/event.cpp index 7a5c358a..e044091a 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/event.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/event.cpp @@ -1,270 +1,240 @@ #include "vn/event.h" + #include "vn/exceptions.h" #if _WIN32 - #include +#include #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - #include - #include - #include +#include +#include +#include #else - #error "Unknown System" +#error "Unknown System" #endif #if __APPLE__ - #include - #include - #include - #include +#include +#include +#include +#include #endif using namespace std; -namespace vn { -namespace xplat { +namespace vn +{ +namespace xplat +{ struct Event::Impl { - #if _WIN32 - HANDLE EventHandle; - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - pthread_mutex_t Mutex; - pthread_cond_t Condition; - bool IsTriggered; - #else - #error "Unknown System" - #endif - - Impl() : - #if _WIN32 - EventHandle(NULL) - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - IsTriggered(false) - #else - #error "Unknown System" - #endif - { - #if _WIN32 - - EventHandle = CreateEvent( - NULL, - false, - false, - NULL); - - if (EventHandle == NULL) - throw unknown_error(); - - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - - pthread_mutex_init(&Mutex, NULL); - - pthread_cond_init(&Condition, NULL); - - #else - - #error "Unknown System" - - #endif - } +#if _WIN32 + HANDLE EventHandle; +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + pthread_mutex_t Mutex; + pthread_cond_t Condition; + bool IsTriggered; +#else +#error "Unknown System" +#endif + + Impl() + : +#if _WIN32 + EventHandle(NULL) +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + IsTriggered(false) +#else +#error "Unknown System" +#endif + { +#if _WIN32 + + EventHandle = CreateEvent(NULL, false, false, NULL); + + if (EventHandle == NULL) throw unknown_error(); + +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + pthread_mutex_init(&Mutex, NULL); + + pthread_cond_init(&Condition, NULL); + +#else + +#error "Unknown System" + +#endif + } }; -Event::Event() : - _pi(new Impl()) -{ } +Event::Event() : _pi(new Impl()) {} Event::~Event() { - #if _WIN32 - CloseHandle(_pi->EventHandle); - #endif +#if _WIN32 + CloseHandle(_pi->EventHandle); +#endif - delete _pi; + delete _pi; } void Event::wait() { - #if _WIN32 - - DWORD result; +#if _WIN32 - result = WaitForSingleObject( - _pi->EventHandle, - INFINITE); + DWORD result; - if (result == WAIT_OBJECT_0) - return; + result = WaitForSingleObject(_pi->EventHandle, INFINITE); - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + if (result == WAIT_OBJECT_0) return; - pthread_mutex_lock(&_pi->Mutex); +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - int errorCode = pthread_cond_wait( - &_pi->Condition, - &_pi->Mutex); + pthread_mutex_lock(&_pi->Mutex); - pthread_mutex_unlock(&_pi->Mutex); + int errorCode = pthread_cond_wait(&_pi->Condition, &_pi->Mutex); - if (errorCode == 0) - return; + pthread_mutex_unlock(&_pi->Mutex); - #else - - #error "Unknown System" + if (errorCode == 0) return; - #endif +#else + +#error "Unknown System" + +#endif - throw unknown_error(); + throw unknown_error(); } Event::WaitResult Event::waitUs(uint32_t timeoutInMicroSec) { - #if _WIN32 - - DWORD result; +#if _WIN32 + + DWORD result; + + result = WaitForSingleObject(_pi->EventHandle, timeoutInMicroSec / 1000); - result = WaitForSingleObject( - _pi->EventHandle, - timeoutInMicroSec / 1000); + if (result == WAIT_OBJECT_0) return WAIT_SIGNALED; - if (result == WAIT_OBJECT_0) - return WAIT_SIGNALED; + if (result == WAIT_TIMEOUT) return WAIT_TIMEDOUT; - if (result == WAIT_TIMEOUT) - return WAIT_TIMEDOUT; +#elif __linux__ || __CYGWIN__ || __QNXNTO__ - #elif __linux__ || __CYGWIN__ || __QNXNTO__ + pthread_mutex_lock(&_pi->Mutex); - pthread_mutex_lock(&_pi->Mutex); + timespec now; + clock_gettime(CLOCK_REALTIME, &now); - timespec now; - clock_gettime(CLOCK_REALTIME, &now); + uint32_t numOfSecs = timeoutInMicroSec / 1000000; + uint32_t numOfNanoseconds = (timeoutInMicroSec % 1000000) * 1000; - uint32_t numOfSecs = timeoutInMicroSec / 1000000; - uint32_t numOfNanoseconds = (timeoutInMicroSec % 1000000) * 1000; + now.tv_sec += numOfSecs; + now.tv_nsec += numOfNanoseconds; - now.tv_sec += numOfSecs; - now.tv_nsec += numOfNanoseconds; + if (now.tv_nsec > 1000000000) { + now.tv_nsec %= 1000000000; + now.tv_sec++; + } - if (now.tv_nsec > 1000000000) - { - now.tv_nsec %= 1000000000; - now.tv_sec++; - } + int errorCode = pthread_cond_timedwait(&_pi->Condition, &_pi->Mutex, &now); - int errorCode = pthread_cond_timedwait( - &_pi->Condition, - &_pi->Mutex, - &now); + pthread_mutex_unlock(&_pi->Mutex); - pthread_mutex_unlock(&_pi->Mutex); + if (errorCode == 0) return WAIT_SIGNALED; - if (errorCode == 0) - return WAIT_SIGNALED; + if (errorCode == ETIMEDOUT) return WAIT_TIMEDOUT; - if (errorCode == ETIMEDOUT) - return WAIT_TIMEDOUT; - - #elif __APPLE__ +#elif __APPLE__ - pthread_mutex_lock(&_pi->Mutex); + pthread_mutex_lock(&_pi->Mutex); - clock_serv_t cclock; - mach_timespec_t mts; - host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock); - clock_get_time(cclock, &mts); - mach_port_deallocate(mach_task_self(), cclock); - - timespec now; - now.tv_sec = mts.tv_sec; - now.tv_nsec = mts.tv_nsec; + clock_serv_t cclock; + mach_timespec_t mts; + host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock); + clock_get_time(cclock, &mts); + mach_port_deallocate(mach_task_self(), cclock); - uint32_t numOfSecs = timeoutInMicroSec / 1000000; - uint32_t numOfNanoseconds = (timeoutInMicroSec % 1000000) * 1000; + timespec now; + now.tv_sec = mts.tv_sec; + now.tv_nsec = mts.tv_nsec; - now.tv_sec += numOfSecs; - now.tv_nsec += numOfNanoseconds; + uint32_t numOfSecs = timeoutInMicroSec / 1000000; + uint32_t numOfNanoseconds = (timeoutInMicroSec % 1000000) * 1000; - if (now.tv_nsec > 1000000000) - { - now.tv_nsec %= 1000000000; - now.tv_sec++; - } + now.tv_sec += numOfSecs; + now.tv_nsec += numOfNanoseconds; - int errorCode = pthread_cond_timedwait( - &_pi->Condition, - &_pi->Mutex, - &now); + if (now.tv_nsec > 1000000000) { + now.tv_nsec %= 1000000000; + now.tv_sec++; + } - pthread_mutex_unlock(&_pi->Mutex); + int errorCode = pthread_cond_timedwait(&_pi->Condition, &_pi->Mutex, &now); - if (errorCode == 0) - return WAIT_SIGNALED; + pthread_mutex_unlock(&_pi->Mutex); - if (errorCode == ETIMEDOUT) - return WAIT_TIMEDOUT; - - #else - - #error "Unknown System" + if (errorCode == 0) return WAIT_SIGNALED; - #endif + if (errorCode == ETIMEDOUT) return WAIT_TIMEDOUT; - throw unknown_error(); +#else + +#error "Unknown System" + +#endif + + throw unknown_error(); } Event::WaitResult Event::waitMs(uint32_t timeoutInMs) { - #if _WIN32 +#if _WIN32 - DWORD result; + DWORD result; - result = WaitForSingleObject( - _pi->EventHandle, - timeoutInMs); + result = WaitForSingleObject(_pi->EventHandle, timeoutInMs); - if (result == WAIT_OBJECT_0) - return WAIT_SIGNALED; + if (result == WAIT_OBJECT_0) return WAIT_SIGNALED; - if (result == WAIT_TIMEOUT) - return WAIT_TIMEDOUT; + if (result == WAIT_TIMEOUT) return WAIT_TIMEDOUT; - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - return waitUs(timeoutInMs * 1000); - - #else - - #error "Unknown System" + return waitUs(timeoutInMs * 1000); - #endif +#else + +#error "Unknown System" + +#endif - throw unknown_error(); + throw unknown_error(); } void Event::signal() { - #if _WIN32 - - if (!SetEvent(_pi->EventHandle)) - throw unknown_error(); +#if _WIN32 - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + if (!SetEvent(_pi->EventHandle)) throw unknown_error(); - pthread_mutex_lock(&_pi->Mutex); +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - _pi->IsTriggered = true; + pthread_mutex_lock(&_pi->Mutex); - pthread_cond_signal(&_pi->Condition); + _pi->IsTriggered = true; - pthread_mutex_unlock(&_pi->Mutex); - - #else - - #error "Unknown System" + pthread_cond_signal(&_pi->Condition); - #endif -} + pthread_mutex_unlock(&_pi->Mutex); +#else + +#error "Unknown System" + +#endif } -} + +} // namespace xplat +} // namespace vn diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/ezasyncdata.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/ezasyncdata.cpp index 4bdccc1c..645695a6 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/ezasyncdata.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/ezasyncdata.cpp @@ -2,122 +2,112 @@ using namespace std; -namespace vn { -namespace sensors { +namespace vn +{ +namespace sensors +{ #if defined(_MSC_VER) - #pragma warning(push) +#pragma warning(push) - // Disable warnings about unused parameters. - #pragma warning(disable:4100) +// Disable warnings about unused parameters. +#pragma warning(disable : 4100) #endif -void EzAsyncData::asyncPacketReceivedHandler(void* userData, protocol::uart::Packet& p, size_t index) +void EzAsyncData::asyncPacketReceivedHandler( + void * userData, protocol::uart::Packet & p, size_t index) { - EzAsyncData* ez = static_cast(userData); + EzAsyncData * ez = static_cast(userData); - CompositeData nd; + CompositeData nd; - #if VN_SUPPORTS_INITIALIZER_LIST - vector d({ &ez->_persistentData, &nd }); - #else - vector d(2); - d[0] = &ez->_persistentData; - d[1] = &nd; - #endif +#if VN_SUPPORTS_INITIALIZER_LIST + vector d({&ez->_persistentData, &nd}); +#else + vector d(2); + d[0] = &ez->_persistentData; + d[1] = &nd; +#endif - ez->_mainCS.enter(); - CompositeData::parse(p, d); - ez->_mainCS.leave(); + ez->_mainCS.enter(); + CompositeData::parse(p, d); + ez->_mainCS.leave(); - ez->_copyCS.enter(); - ez->_nextData = nd; - ez->_copyCS.leave(); + ez->_copyCS.enter(); + ez->_nextData = nd; + ez->_copyCS.leave(); - ez->_newDataEvent.signal(); + ez->_newDataEvent.signal(); } -#if defined (_MSC_VER) - #pragma warning(pop) +#if defined(_MSC_VER) +#pragma warning(pop) #endif -EzAsyncData::EzAsyncData(VnSensor* sensor) : - _sensor(sensor) +EzAsyncData::EzAsyncData(VnSensor * sensor) : _sensor(sensor) { - _sensor->registerAsyncPacketReceivedHandler(this, &EzAsyncData::asyncPacketReceivedHandler); + _sensor->registerAsyncPacketReceivedHandler(this, &EzAsyncData::asyncPacketReceivedHandler); } -VnSensor* EzAsyncData::sensor() -{ - return _sensor; -} +VnSensor * EzAsyncData::sensor() { return _sensor; } -EzAsyncData* EzAsyncData::connect(string portName, uint32_t baudrate) +EzAsyncData * EzAsyncData::connect(string portName, uint32_t baudrate) { - VnSensor* s = new VnSensor(); - - try - { - s->connect(portName, baudrate); - } - catch (exception e) - { - // Added to clean up the pointer that would leak here. - // Rethrow the same exception. - delete s; - s = NULL; - throw; - } - - return new EzAsyncData(s); + VnSensor * s = new VnSensor(); + + try { + s->connect(portName, baudrate); + } catch (exception e) { + // Added to clean up the pointer that would leak here. + // Rethrow the same exception. + delete s; + s = NULL; + throw; + } + + return new EzAsyncData(s); } void EzAsyncData::disconnect() { - _sensor->unregisterAsyncPacketReceivedHandler(); + _sensor->unregisterAsyncPacketReceivedHandler(); - _sensor->disconnect(); + _sensor->disconnect(); } CompositeData EzAsyncData::currentData() { - CompositeData cd; + CompositeData cd; - _mainCS.enter(); - cd = _persistentData; - _mainCS.leave(); + _mainCS.enter(); + cd = _persistentData; + _mainCS.leave(); - return CompositeData(cd); + return CompositeData(cd); } -CompositeData EzAsyncData::getNextData() -{ - return getNextData(0xFFFFFFFF); -} +CompositeData EzAsyncData::getNextData() { return getNextData(0xFFFFFFFF); } CompositeData EzAsyncData::getNextData(int timeoutMs) { - if (_newDataEvent.waitMs(timeoutMs) == xplat::Event::WAIT_TIMEDOUT) - throw timeout(); + if (_newDataEvent.waitMs(timeoutMs) == xplat::Event::WAIT_TIMEDOUT) throw timeout(); - CompositeData cd; + CompositeData cd; - _copyCS.enter(); - cd = _nextData; - _copyCS.leave(); + _copyCS.enter(); + cd = _nextData; + _copyCS.leave(); - return cd; + return cd; } EzAsyncData::~EzAsyncData() { - if (NULL != _sensor) - { - delete _sensor; - _sensor = NULL; - } + if (NULL != _sensor) { + delete _sensor; + _sensor = NULL; + } } - -} -} +} // namespace sensors +} // namespace vn diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/memoryport.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/memoryport.cpp index 8b5e3917..51145781 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/memoryport.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/memoryport.cpp @@ -1,186 +1,171 @@ #include "vn/memoryport.h" -#include "vn/criticalsection.h" -#include "vn/exceptions.h" #include +#include "vn/criticalsection.h" +#include "vn/exceptions.h" + using namespace std; using namespace vn::xplat; -namespace vn { -namespace util { +namespace vn +{ +namespace util +{ struct MemoryPort::Impl { - // Indiates if the serial port is open. - bool IsOpen; + // Indiates if the serial port is open. + bool IsOpen; - // Critical section for registering, unregistering, and notifying observers - // of events. - CriticalSection ObserversCriticalSection; + // Critical section for registering, unregistering, and notifying observers + // of events. + CriticalSection ObserversCriticalSection; - DataReceivedHandler _dataReceivedHandler; - void* _dataReceivedUserData; - DataWrittenHandler _dataWrittenHandler; - void* _dataWrittenUserData; + DataReceivedHandler _dataReceivedHandler; + void * _dataReceivedUserData; + DataWrittenHandler _dataWrittenHandler; + void * _dataWrittenUserData; - const uint8_t* DataAvailableForRead; + const uint8_t * DataAvailableForRead; - size_t DataAvailableForReadLength; + size_t DataAvailableForReadLength; - MemoryPort *BackReference; + MemoryPort * BackReference; - explicit Impl(MemoryPort* backReference) : - IsOpen(false), - _dataReceivedHandler(NULL), - _dataReceivedUserData(NULL), - _dataWrittenHandler(NULL), - _dataWrittenUserData(NULL), - DataAvailableForRead(NULL), - DataAvailableForReadLength(0), - BackReference(backReference) - { } + explicit Impl(MemoryPort * backReference) + : IsOpen(false), + _dataReceivedHandler(NULL), + _dataReceivedUserData(NULL), + _dataWrittenHandler(NULL), + _dataWrittenUserData(NULL), + DataAvailableForRead(NULL), + DataAvailableForReadLength(0), + BackReference(backReference) + { + } - void OnDataReceived() - { - if (_dataReceivedHandler == NULL) - return; + void OnDataReceived() + { + if (_dataReceivedHandler == NULL) return; - ObserversCriticalSection.enter(); + ObserversCriticalSection.enter(); - _dataReceivedHandler(_dataReceivedUserData); + _dataReceivedHandler(_dataReceivedUserData); - ObserversCriticalSection.leave(); - } + ObserversCriticalSection.leave(); + } }; #if defined(_MSC_VER) - #pragma warning(push) - #pragma warning(disable:4355) +#pragma warning(push) +#pragma warning(disable : 4355) #endif -MemoryPort::MemoryPort() : - _pi(new Impl(this)) -{ -} +MemoryPort::MemoryPort() : _pi(new Impl(this)) {} -#if defined (_MSC_VER) - #pragma warning(pop) +#if defined(_MSC_VER) +#pragma warning(pop) #endif -MemoryPort::~MemoryPort() -{ - delete _pi; -} +MemoryPort::~MemoryPort() { delete _pi; } void MemoryPort::open() { - if (_pi->IsOpen) - throw invalid_operation(); + if (_pi->IsOpen) throw invalid_operation(); - _pi->IsOpen = true; + _pi->IsOpen = true; } void MemoryPort::close() { - if (!_pi->IsOpen) - throw invalid_operation(); + if (!_pi->IsOpen) throw invalid_operation(); - _pi->IsOpen = false; + _pi->IsOpen = false; } -bool MemoryPort::isOpen() -{ - return _pi->IsOpen; -} +bool MemoryPort::isOpen() { return _pi->IsOpen; } void MemoryPort::write(const char data[], size_t length) { - if (!_pi->IsOpen) - throw invalid_operation(); + if (!_pi->IsOpen) throw invalid_operation(); - if (_pi->_dataWrittenHandler != NULL) - _pi->_dataWrittenHandler(_pi->_dataWrittenUserData, data, length); + if (_pi->_dataWrittenHandler != NULL) + _pi->_dataWrittenHandler(_pi->_dataWrittenUserData, data, length); } -void MemoryPort::read(char dataBuffer[], size_t numOfBytesToRead, size_t &numOfBytesActuallyRead) +void MemoryPort::read(char dataBuffer[], size_t numOfBytesToRead, size_t & numOfBytesActuallyRead) { - if (!_pi->IsOpen) - throw invalid_operation(); + if (!_pi->IsOpen) throw invalid_operation(); - if (numOfBytesToRead < _pi->DataAvailableForReadLength) - // Don't support less than full read for now. - throw not_implemented(); + if (numOfBytesToRead < _pi->DataAvailableForReadLength) + // Don't support less than full read for now. + throw not_implemented(); - copy(_pi->DataAvailableForRead, _pi->DataAvailableForRead + _pi->DataAvailableForReadLength, dataBuffer); + copy( + _pi->DataAvailableForRead, _pi->DataAvailableForRead + _pi->DataAvailableForReadLength, + dataBuffer); - numOfBytesActuallyRead = _pi->DataAvailableForReadLength; + numOfBytesActuallyRead = _pi->DataAvailableForReadLength; } -void MemoryPort::registerDataReceivedHandler(void* userData, DataReceivedHandler handler) +void MemoryPort::registerDataReceivedHandler(void * userData, DataReceivedHandler handler) { - if (_pi->_dataReceivedHandler != NULL) - throw invalid_operation(); + if (_pi->_dataReceivedHandler != NULL) throw invalid_operation(); - _pi->ObserversCriticalSection.enter(); + _pi->ObserversCriticalSection.enter(); - _pi->_dataReceivedHandler = handler; - _pi->_dataReceivedUserData = userData; + _pi->_dataReceivedHandler = handler; + _pi->_dataReceivedUserData = userData; - _pi->ObserversCriticalSection.leave(); + _pi->ObserversCriticalSection.leave(); } void MemoryPort::unregisterDataReceivedHandler() { - if (_pi->_dataReceivedHandler == NULL) - throw invalid_operation(); + if (_pi->_dataReceivedHandler == NULL) throw invalid_operation(); - _pi->ObserversCriticalSection.enter(); + _pi->ObserversCriticalSection.enter(); - _pi->_dataReceivedHandler = NULL; - _pi->_dataReceivedUserData = NULL; + _pi->_dataReceivedHandler = NULL; + _pi->_dataReceivedUserData = NULL; - _pi->ObserversCriticalSection.leave(); + _pi->ObserversCriticalSection.leave(); } -void MemoryPort::registerDataWrittenHandler(void* userData, DataWrittenHandler handler) +void MemoryPort::registerDataWrittenHandler(void * userData, DataWrittenHandler handler) { - if (_pi->_dataWrittenHandler != NULL) - throw invalid_operation(); + if (_pi->_dataWrittenHandler != NULL) throw invalid_operation(); - _pi->_dataWrittenHandler = handler; - _pi->_dataWrittenUserData = userData; + _pi->_dataWrittenHandler = handler; + _pi->_dataWrittenUserData = userData; } void MemoryPort::unregisterDataWrittenHandler() { - if (_pi->_dataWrittenHandler == NULL) - throw invalid_operation(); + if (_pi->_dataWrittenHandler == NULL) throw invalid_operation(); - _pi->_dataWrittenHandler = NULL; - _pi->_dataWrittenUserData = NULL; + _pi->_dataWrittenHandler = NULL; + _pi->_dataWrittenUserData = NULL; } void MemoryPort::SendDataBackDoor(const uint8_t data[], size_t length) { - _pi->DataAvailableForRead = data; - _pi->DataAvailableForReadLength = length; + _pi->DataAvailableForRead = data; + _pi->DataAvailableForReadLength = length; - _pi->OnDataReceived(); + _pi->OnDataReceived(); } -void MemoryPort::SendDataBackDoor( - const char data[], - std::size_t length) +void MemoryPort::SendDataBackDoor(const char data[], std::size_t length) { - SendDataBackDoor(reinterpret_cast(data), length); + SendDataBackDoor(reinterpret_cast(data), length); } -void MemoryPort::SendDataBackDoor( - const std::string data) +void MemoryPort::SendDataBackDoor(const std::string data) { - SendDataBackDoor(data.c_str(), data.length()); + SendDataBackDoor(data.c_str(), data.length()); } -} -} +} // namespace util +} // namespace vn diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/packet.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/packet.cpp index 79cbed19..b0ba38ef 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/packet.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/packet.cpp @@ -1,7 +1,8 @@ #include "vn/packet.h" -#include "vn/utilities.h" -#include "vn/error_detection.h" + #include "vn/compiler.h" +#include "vn/error_detection.h" +#include "vn/utilities.h" // TODO : Remove all C style coding where possible #include @@ -10,26 +11,24 @@ // the user's compiler is not C++11 compliant #include #include -#include -#include #include - -#include +#include +#include #if PYTHON - #include "python.hpp" - #include "util.h" - namespace bp = boost::python; +#include "python.hpp" +#include "util.h" +namespace bp = boost::python; #endif -#define NEXT result = getNextData(_data, parseIndex); \ - if (result == NULL) \ - return; +#define NEXT \ + result = getNextData(_data, parseIndex); \ + if (result == NULL) return; #define ATOFF static_cast(std::atof(result)) #define ATOFD std::atof(result) #define ATOU32 static_cast(std::atoi(result)) -#define ATOU16X ((uint16_t) std::strtoul(result, NULL, 16)) +#define ATOU16X ((uint16_t)std::strtoul(result, NULL, 16)) #define ATOU16 static_cast(std::atoi(result)) #define ATOU8 static_cast(std::atoi(result)) @@ -38,2516 +37,2429 @@ using namespace vn::math; //using namespace vn::xplat; using namespace vn::data::integrity; -namespace vn { -namespace protocol { -namespace uart { - -char* vnstrtok(char* str, size_t& startIndex); - -const unsigned char Packet::BinaryGroupLengths[sizeof(uint8_t)*8][sizeof(uint16_t)*15] = { - { 8, 8, 8, 12, 16, 12, 24, 12, 12, 24, 20, 28, 2, 4, 8}, // Group 1 - { 8, 8, 8, 2, 8, 8, 8, 4, 4, 1, 0, 0, 0, 0, 0}, // Group 2 - { 2, 12, 12, 12, 4, 4, 16, 12, 12, 12, 12, 2, 40, 0, 0}, // Group 3 - { 8, 8, 2, 1, 1, 24, 24, 12, 12, 12, 4, 4, 2, 28, 0}, // Group 4 - { 2, 12, 16, 36, 12, 12, 12, 12, 12, 12, 28, 24, 12, 0, 0}, // Group 5 - { 2, 24, 24, 12, 12, 12, 12, 12, 12, 4, 4, 68, 64, 0, 0}, // Group 6 - { 8, 8, 2, 1, 1, 24, 24, 12, 12, 12, 4, 4, 2, 28, 0}, // Group 7 - { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} // Invalid group +namespace vn +{ +namespace protocol +{ +namespace uart +{ + +char * vnstrtok(char * str, size_t & startIndex); + +const unsigned char Packet::BinaryGroupLengths[sizeof(uint8_t) * 8][sizeof(uint16_t) * 15] = { + {8, 8, 8, 12, 16, 12, 24, 12, 12, 24, 20, 28, 2, 4, 8}, // Group 1 + {8, 8, 8, 2, 8, 8, 8, 4, 4, 1, 0, 0, 0, 0, 0}, // Group 2 + {2, 12, 12, 12, 4, 4, 16, 12, 12, 12, 12, 2, 40, 0, 0}, // Group 3 + {8, 8, 2, 1, 1, 24, 24, 12, 12, 12, 4, 4, 2, 28, 0}, // Group 4 + {2, 12, 16, 36, 12, 12, 12, 12, 12, 12, 28, 24, 12, 0, 0}, // Group 5 + {2, 24, 24, 12, 12, 12, 12, 12, 12, 4, 4, 68, 64, 0, 0}, // Group 6 + {8, 8, 2, 1, 1, 24, 24, 12, 12, 12, 4, 4, 2, 28, 0}, // Group 7 + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} // Invalid group }; -Packet::Packet() : - _isPacketDataMine(false), - _length(0), - _data(NULL) -{ -} +Packet::Packet() : _isPacketDataMine(false), _length(0), _data(NULL) {} -Packet::Packet(char const* packet, size_t length) : - _isPacketDataMine(true), - _length(length), - _data(new char[length]), - _curExtractLoc(0) +Packet::Packet(char const * packet, size_t length) +: _isPacketDataMine(true), _length(length), _data(new char[length]), _curExtractLoc(0) { - std::memcpy(_data, packet, length); + std::memcpy(_data, packet, length); } -Packet::Packet(string packet) : - _isPacketDataMine(true), - _length(packet.size()), - _data(new char[packet.size()]), - _curExtractLoc(0) +Packet::Packet(string packet) +: _isPacketDataMine(true), _length(packet.size()), _data(new char[packet.size()]), _curExtractLoc(0) { - std::memcpy(_data, packet.c_str(), packet.size()); + std::memcpy(_data, packet.c_str(), packet.size()); } -Packet::Packet(Packet const& toCopy) : - _isPacketDataMine(true), - _length(toCopy._length), - _data(new char[toCopy._length]), - _curExtractLoc(0) +Packet::Packet(Packet const & toCopy) +: _isPacketDataMine(true), + _length(toCopy._length), + _data(new char[toCopy._length]), + _curExtractLoc(0) { - std::memcpy(_data, toCopy._data, toCopy._length); + std::memcpy(_data, toCopy._data, toCopy._length); } Packet::~Packet() { - if (_isPacketDataMine) - { - delete[] _data; - } + if (_isPacketDataMine) { + delete[] _data; + } } -Packet& Packet::operator=(Packet const& from) +Packet & Packet::operator=(Packet const & from) { - if (_isPacketDataMine) - { - delete[] _data; - } - + if (_isPacketDataMine) { + delete[] _data; + } - _isPacketDataMine = true; - _data = new char[from._length]; - _length = from._length; - _curExtractLoc = from._curExtractLoc; + _isPacketDataMine = true; + _data = new char[from._length]; + _length = from._length; + _curExtractLoc = from._curExtractLoc; - std::memcpy(_data, from._data, from._length); + std::memcpy(_data, from._data, from._length); - return *this; + return *this; } -string Packet::datastr() -{ - return string(_data, _length); -} +string Packet::datastr() { return string(_data, _length); } Packet::Type Packet::type() { - if (_length < 1) - throw invalid_operation("Packet does not contain any data."); + if (_length < 1) throw invalid_operation("Packet does not contain any data."); - // #TODO : Added this for the if check below due to type checking - // Sometimes the 0xFA is compared as a signed char and - // sometimes as an unsigned char. - const unsigned char binary_indicator = 0XFA; - // Since this is not a pointer use the functional cast for C++ rather than the C style cast - const unsigned char data_zero = static_cast(_data[0]); //unsigned char(_data[0]); + // #TODO : Added this for the if check below due to type checking + // Sometimes the 0xFA is compared as a signed char and + // sometimes as an unsigned char. + const unsigned char binary_indicator = 0XFA; + // Since this is not a pointer use the functional cast for C++ rather than the C style cast + const unsigned char data_zero = static_cast(_data[0]); //unsigned char(_data[0]); - if (_data[0] == '$') - return TYPE_ASCII; - //if (_data[0] == binary_indicator) - //if (_data[0] == 0xFA) - if (data_zero == binary_indicator) - return TYPE_BINARY; + if (_data[0] == '$') return TYPE_ASCII; + //if (_data[0] == binary_indicator) + //if (_data[0] == 0xFA) + if (data_zero == binary_indicator) return TYPE_BINARY; - return TYPE_UNKNOWN; + return TYPE_UNKNOWN; } bool Packet::isValid() { - if (_length < 7) // minumum binary packet is 7 bytes, minimum ASCII is 8 bytes - return false; - - if (type() == TYPE_ASCII) - { - // First determine if this packet does not have a checksum or CRC. - if (_data[_length - 3] == 'X' && _data[_length - 4] == 'X') - return true; - - // First determine if this packet has an 8-bit checksum or a 16-bit CRC. - if (_data[_length - 5] == '*') - { - // Appears we have an 8-bit checksum packet. - uint8_t expectedChecksum = toUint8FromHexStr(_data + _length - 4); - - uint8_t computedChecksum = Checksum8::compute(_data + 1, _length - 6); - - return expectedChecksum == computedChecksum; - } - else if (_data[_length - 7] == '*') - { - // Appears we have a 16-bit CRC packet. - uint16_t packetCrc = to_uint16_from_hexstr(_data + _length - 6); - - uint16_t computedCrc = Crc16::compute(_data + 1, _length - 8); - - return packetCrc == computedCrc; - } - else if (_data[_length - 6] == '*') // ASCII Responses curing bootloader mode - { - // Appears we have a 16-bit CRC packet. - uint16_t packetCrc = to_uint16_from_hexstr(_data + _length - 5); - - uint16_t computedCrc = Crc16::compute(_data + 1, _length - 7); - - return packetCrc == computedCrc; - } - else - { - // Don't know what we have. - return false; - } - } - else if (type() == TYPE_BINARY) - { - uint16_t computedCrc = Crc16::compute(_data + 1, _length - 1); - - return computedCrc == 0; - } - else - { - // Check for special case for switching to bootloader mode - char bootloadSignature[] = "VectorNav Bootloader"; - if (strncmp(_data, bootloadSignature, sizeof(bootloadSignature) - 1) == 0) - { - return true; - } - - throw not_implemented(); - } -} - -bool Packet::isError() -{ - return std::strncmp(_data + 3, "ERR", 3) == 0; + if (_length < 7) // minumum binary packet is 7 bytes, minimum ASCII is 8 bytes + return false; + + if (type() == TYPE_ASCII) { + // First determine if this packet does not have a checksum or CRC. + if (_data[_length - 3] == 'X' && _data[_length - 4] == 'X') return true; + + // First determine if this packet has an 8-bit checksum or a 16-bit CRC. + if (_data[_length - 5] == '*') { + // Appears we have an 8-bit checksum packet. + uint8_t expectedChecksum = toUint8FromHexStr(_data + _length - 4); + + uint8_t computedChecksum = Checksum8::compute(_data + 1, _length - 6); + + return expectedChecksum == computedChecksum; + } else if (_data[_length - 7] == '*') { + // Appears we have a 16-bit CRC packet. + uint16_t packetCrc = to_uint16_from_hexstr(_data + _length - 6); + + uint16_t computedCrc = Crc16::compute(_data + 1, _length - 8); + + return packetCrc == computedCrc; + } else if (_data[_length - 6] == '*') // ASCII Responses curing bootloader mode + { + // Appears we have a 16-bit CRC packet. + uint16_t packetCrc = to_uint16_from_hexstr(_data + _length - 5); + + uint16_t computedCrc = Crc16::compute(_data + 1, _length - 7); + + return packetCrc == computedCrc; + } else { + // Don't know what we have. + return false; + } + } else if (type() == TYPE_BINARY) { + uint16_t computedCrc = Crc16::compute(_data + 1, _length - 1); + + return computedCrc == 0; + } else { + // Check for special case for switching to bootloader mode + char bootloadSignature[] = "VectorNav Bootloader"; + if (strncmp(_data, bootloadSignature, sizeof(bootloadSignature) - 1) == 0) { + return true; + } + + throw not_implemented(); + } } +bool Packet::isError() { return std::strncmp(_data + 3, "ERR", 3) == 0; } + bool Packet::isResponse() { - if (std::strncmp(_data + 3, "WRG", 3) == 0) - return true; - if (std::strncmp(_data + 3, "RRG", 3) == 0) - return true; - if (std::strncmp(_data + 3, "WNV", 3) == 0) - return true; - if (std::strncmp(_data + 3, "RFS", 3) == 0) - return true; - if (std::strncmp(_data + 3, "RST", 3) == 0) - return true; - if (std::strncmp(_data + 3, "FWU", 3) == 0) - return true; - if (std::strncmp(_data + 3, "CMD", 3) == 0) - return true; - if (std::strncmp(_data + 3, "ASY", 3) == 0) - return true; - if (std::strncmp(_data + 3, "TAR", 3) == 0) - return true; - if (std::strncmp(_data + 3, "KMD", 3) == 0) - return true; - if (std::strncmp(_data + 3, "KAD", 3) == 0) - return true; - if (std::strncmp(_data + 3, "SGB", 3) == 0) - return true; - if (std::strncmp(_data + 3, "DBS", 3) == 0) - return true; - if (std::strncmp(_data + 3, "MCU", 3) == 0) - return true; - if (std::strncmp(_data + 3, "SBL", 3) == 0) - return true; - if (std::strncmp(_data + 3, "SPS", 3) == 0) - return true; - if (std::strncmp(_data + 3, "BLD", 3) == 0) - return true; - if (std::strncmp(_data, "VectorNav Bootloader", 20) == 0) - return true; - - return false; + if (std::strncmp(_data + 3, "WRG", 3) == 0) return true; + if (std::strncmp(_data + 3, "RRG", 3) == 0) return true; + if (std::strncmp(_data + 3, "WNV", 3) == 0) return true; + if (std::strncmp(_data + 3, "RFS", 3) == 0) return true; + if (std::strncmp(_data + 3, "RST", 3) == 0) return true; + if (std::strncmp(_data + 3, "FWU", 3) == 0) return true; + if (std::strncmp(_data + 3, "CMD", 3) == 0) return true; + if (std::strncmp(_data + 3, "ASY", 3) == 0) return true; + if (std::strncmp(_data + 3, "TAR", 3) == 0) return true; + if (std::strncmp(_data + 3, "KMD", 3) == 0) return true; + if (std::strncmp(_data + 3, "KAD", 3) == 0) return true; + if (std::strncmp(_data + 3, "SGB", 3) == 0) return true; + if (std::strncmp(_data + 3, "DBS", 3) == 0) return true; + if (std::strncmp(_data + 3, "MCU", 3) == 0) return true; + if (std::strncmp(_data + 3, "SBL", 3) == 0) return true; + if (std::strncmp(_data + 3, "SPS", 3) == 0) return true; + if (std::strncmp(_data + 3, "BLD", 3) == 0) return true; + if (std::strncmp(_data, "VectorNav Bootloader", 20) == 0) return true; + + return false; } bool Packet::isAsciiAsync() { - // Pointer to the unique asynchronous data type identifier. - char* pAT = _data + 3; - - if (strncmp(pAT, "YPR", 3) == 0) - return true; - if (strncmp(pAT, "QTN", 3) == 0) - return true; - #ifdef INTERNAL - if (strncmp(pAT, "QTM", 3) == 0) - return true; - if (strncmp(pAT, "QTA", 3) == 0) - return true; - if (strncmp(pAT, "QTR", 3) == 0) - return true; - if (strncmp(pAT, "QMA", 3) == 0) - return true; - if (strncmp(pAT, "QAR", 3) == 0) - return true; - #endif - if (strncmp(pAT, "QMR", 3) == 0) - return true; - #ifdef INTERNAL - if (strncmp(pAT, "DCM", 3) == 0) - return true; - #endif - if (strncmp(pAT, "MAG", 3) == 0) - return true; - if (strncmp(pAT, "ACC", 3) == 0) - return true; - if (strncmp(pAT, "GYR", 3) == 0) - return true; - if (strncmp(pAT, "MAR", 3) == 0) - return true; - if (strncmp(pAT, "YMR", 3) == 0) - return true; - #ifdef INTERNAL - if (strncmp(pAT, "YCM", 3) == 0) - return true; - #endif - if (strncmp(pAT, "YBA", 3) == 0) - return true; - if (strncmp(pAT, "YIA", 3) == 0) - return true; - #ifdef INTERNAL - if (strncmp(pAT, "ICM", 3) == 0) - return true; - #endif - if (strncmp(pAT, "IMU", 3) == 0) - return true; - if (strncmp(pAT, "GPS", 3) == 0) - return true; - if (strncmp(pAT, "GPE", 3) == 0) - return true; - if (strncmp(pAT, "INS", 3) == 0) - return true; - if (strncmp(pAT, "INE", 3) == 0) - return true; - if (strncmp(pAT, "ISL", 3) == 0) - return true; - if (strncmp(pAT, "ISE", 3) == 0) - return true; - if (strncmp(pAT, "DTV", 3) == 0) - return true; - if(strncmp(pAT, "G2S", 3) == 0) - return true; - if(strncmp(pAT, "G2E", 3) == 0) - return true; - #ifdef INTERNAL - if (strncmp(pAT, "RAW", 3) == 0) - return true; - if (strncmp(pAT, "CMV", 3) == 0) - return true; - if (strncmp(pAT, "STV", 3) == 0) - return true; - if (strncmp(pAT, "COV", 3) == 0) - return true; - #endif - if (strncmp(pAT, "RTK", 3) == 0) - return true; - - return false; + // Pointer to the unique asynchronous data type identifier. + char * pAT = _data + 3; + + if (strncmp(pAT, "YPR", 3) == 0) return true; + if (strncmp(pAT, "QTN", 3) == 0) return true; +#ifdef INTERNAL + if (strncmp(pAT, "QTM", 3) == 0) return true; + if (strncmp(pAT, "QTA", 3) == 0) return true; + if (strncmp(pAT, "QTR", 3) == 0) return true; + if (strncmp(pAT, "QMA", 3) == 0) return true; + if (strncmp(pAT, "QAR", 3) == 0) return true; +#endif + if (strncmp(pAT, "QMR", 3) == 0) return true; +#ifdef INTERNAL + if (strncmp(pAT, "DCM", 3) == 0) return true; +#endif + if (strncmp(pAT, "MAG", 3) == 0) return true; + if (strncmp(pAT, "ACC", 3) == 0) return true; + if (strncmp(pAT, "GYR", 3) == 0) return true; + if (strncmp(pAT, "MAR", 3) == 0) return true; + if (strncmp(pAT, "YMR", 3) == 0) return true; +#ifdef INTERNAL + if (strncmp(pAT, "YCM", 3) == 0) return true; +#endif + if (strncmp(pAT, "YBA", 3) == 0) return true; + if (strncmp(pAT, "YIA", 3) == 0) return true; +#ifdef INTERNAL + if (strncmp(pAT, "ICM", 3) == 0) return true; +#endif + if (strncmp(pAT, "IMU", 3) == 0) return true; + if (strncmp(pAT, "GPS", 3) == 0) return true; + if (strncmp(pAT, "GPE", 3) == 0) return true; + if (strncmp(pAT, "INS", 3) == 0) return true; + if (strncmp(pAT, "INE", 3) == 0) return true; + if (strncmp(pAT, "ISL", 3) == 0) return true; + if (strncmp(pAT, "ISE", 3) == 0) return true; + if (strncmp(pAT, "DTV", 3) == 0) return true; + if (strncmp(pAT, "G2S", 3) == 0) return true; + if (strncmp(pAT, "G2E", 3) == 0) return true; +#ifdef INTERNAL + if (strncmp(pAT, "RAW", 3) == 0) return true; + if (strncmp(pAT, "CMV", 3) == 0) return true; + if (strncmp(pAT, "STV", 3) == 0) return true; + if (strncmp(pAT, "COV", 3) == 0) return true; +#endif + if (strncmp(pAT, "RTK", 3) == 0) return true; + + return false; } bool Packet::isBootloader() { - if (std::strncmp(_data + 3, "BLD", 3) == 0) - return true; - if (std::strncmp(_data, "VectorNav Bootloader", 20) == 0) - return true; + if (std::strncmp(_data + 3, "BLD", 3) == 0) return true; + if (std::strncmp(_data, "VectorNav Bootloader", 20) == 0) return true; - return false; + return false; } AsciiAsync Packet::determineAsciiAsyncType() { - // Pointer to the unique asynchronous data type identifier. - char* pAT = _data + 3; - - if (strncmp(pAT, "YPR", 3) == 0) - return VNYPR; - if (strncmp(pAT, "QTN", 3) == 0) - return VNQTN; - #ifdef INTERNAL - if (strncmp(pAT, "QTM", 3) == 0) - return VNQTM; - if (strncmp(pAT, "QTA", 3) == 0) - return VNQTA; - if (strncmp(pAT, "QTR", 3) == 0) - return VNQTR; - if (strncmp(pAT, "QMA", 3) == 0) - return VNQMA; - if (strncmp(pAT, "QAR", 3) == 0) - return VNQAR; - #endif - if (strncmp(pAT, "QMR", 3) == 0) - return VNQMR; - #ifdef INTERNAL - if (strncmp(pAT, "DCM", 3) == 0) - return VNDCM; - #endif - if (strncmp(pAT, "MAG", 3) == 0) - return VNMAG; - if (strncmp(pAT, "ACC", 3) == 0) - return VNACC; - if (strncmp(pAT, "GYR", 3) == 0) - return VNGYR; - if (strncmp(pAT, "MAR", 3) == 0) - return VNMAR; - if (strncmp(pAT, "YMR", 3) == 0) - return VNYMR; - #ifdef INTERNAL - if (strncmp(pAT, "YCM", 3) == 0) - return VNYCM; - #endif - if (strncmp(pAT, "YBA", 3) == 0) - return VNYBA; - if (strncmp(pAT, "YIA", 3) == 0) - return VNYIA; - #ifdef INTERNAL - if (strncmp(pAT, "ICM", 3) == 0) - return VNICM; - #endif - if (strncmp(pAT, "IMU", 3) == 0) - return VNIMU; - if (strncmp(pAT, "GPS", 3) == 0) - return VNGPS; - if (strncmp(pAT, "GPE", 3) == 0) - return VNGPE; - if (strncmp(pAT, "INS", 3) == 0) - return VNINS; - if (strncmp(pAT, "INE", 3) == 0) - return VNINE; - if (strncmp(pAT, "ISL", 3) == 0) - return VNISL; - if (strncmp(pAT, "ISE", 3) == 0) - return VNISE; - if (strncmp(pAT, "DTV", 3) == 0) - return VNDTV; - if(strncmp(pAT, "G2S", 3) == 0) - return VNG2S; - if(strncmp(pAT, "G2E", 3) == 0) - return VNG2E; + // Pointer to the unique asynchronous data type identifier. + char * pAT = _data + 3; + + if (strncmp(pAT, "YPR", 3) == 0) return VNYPR; + if (strncmp(pAT, "QTN", 3) == 0) return VNQTN; #ifdef INTERNAL - if (strncmp(pAT, "RAW", 3) == 0) - return VNRAW; - if (strncmp(pAT, "CMV", 3) == 0) - return VNCMV; - if (strncmp(pAT, "STV", 3) == 0) - return VNSTV; - if (strncmp(pAT, "COV", 3) == 0) - return VNCOV; - #endif - else - throw unknown_error(); -} - -bool Packet::isCompatible(CommonGroup commonGroup, TimeGroup timeGroup, ImuGroup imuGroup, GpsGroup gpsGroup, AttitudeGroup attitudeGroup, InsGroup insGroup, GpsGroup gps2Group) -{ - // First make sure the appropriate groups are specified. - uint8_t groups = _data[1]; - char *curField = _data + 2; - - if (commonGroup) - { - if(*reinterpret_cast(curField) != commonGroup) + if (strncmp(pAT, "QTM", 3) == 0) return VNQTM; + if (strncmp(pAT, "QTA", 3) == 0) return VNQTA; + if (strncmp(pAT, "QTR", 3) == 0) return VNQTR; + if (strncmp(pAT, "QMA", 3) == 0) return VNQMA; + if (strncmp(pAT, "QAR", 3) == 0) return VNQAR; +#endif + if (strncmp(pAT, "QMR", 3) == 0) return VNQMR; +#ifdef INTERNAL + if (strncmp(pAT, "DCM", 3) == 0) return VNDCM; +#endif + if (strncmp(pAT, "MAG", 3) == 0) return VNMAG; + if (strncmp(pAT, "ACC", 3) == 0) return VNACC; + if (strncmp(pAT, "GYR", 3) == 0) return VNGYR; + if (strncmp(pAT, "MAR", 3) == 0) return VNMAR; + if (strncmp(pAT, "YMR", 3) == 0) return VNYMR; +#ifdef INTERNAL + if (strncmp(pAT, "YCM", 3) == 0) return VNYCM; +#endif + if (strncmp(pAT, "YBA", 3) == 0) return VNYBA; + if (strncmp(pAT, "YIA", 3) == 0) return VNYIA; +#ifdef INTERNAL + if (strncmp(pAT, "ICM", 3) == 0) return VNICM; +#endif + if (strncmp(pAT, "IMU", 3) == 0) return VNIMU; + if (strncmp(pAT, "GPS", 3) == 0) return VNGPS; + if (strncmp(pAT, "GPE", 3) == 0) return VNGPE; + if (strncmp(pAT, "INS", 3) == 0) return VNINS; + if (strncmp(pAT, "INE", 3) == 0) return VNINE; + if (strncmp(pAT, "ISL", 3) == 0) return VNISL; + if (strncmp(pAT, "ISE", 3) == 0) return VNISE; + if (strncmp(pAT, "DTV", 3) == 0) return VNDTV; + if (strncmp(pAT, "G2S", 3) == 0) return VNG2S; + if (strncmp(pAT, "G2E", 3) == 0) return VNG2E; +#ifdef INTERNAL + if (strncmp(pAT, "RAW", 3) == 0) return VNRAW; + if (strncmp(pAT, "CMV", 3) == 0) return VNCMV; + if (strncmp(pAT, "STV", 3) == 0) return VNSTV; + if (strncmp(pAT, "COV", 3) == 0) return VNCOV; +#endif + else + throw unknown_error(); +} + +bool Packet::isCompatible( + CommonGroup commonGroup, TimeGroup timeGroup, ImuGroup imuGroup, GpsGroup gpsGroup, + AttitudeGroup attitudeGroup, InsGroup insGroup, GpsGroup gps2Group) +{ + // First make sure the appropriate groups are specified. + uint8_t groups = _data[1]; + char * curField = _data + 2; + + if (commonGroup) { + if (*reinterpret_cast(curField) != commonGroup) // Not the expected collection of field data types. - return false; - - curField += 2; - } - else if (groups & 0x01) - { - // There is unexpected Common Group data. - return false; - } - - if (timeGroup) - { - if (*reinterpret_cast(curField) != timeGroup) - // Not the expected collection of field data types. - return false; - - curField += 2; - } - else if (groups & 0x02) - { - // There is unexpected Time Group data. - return false; - } - - if (imuGroup) - { - if (*reinterpret_cast(curField) != imuGroup) - // Not the expected collection of field data types. - return false; - - curField += 2; - } - else if (groups & 0x04) - { - // There is unexpected IMU Group data. - return false; - } - - if (gpsGroup) - { - if (*reinterpret_cast(curField) != gpsGroup) - // Not the expected collection of field data types. - return false; - - curField += 2; - } - else if (groups & 0x08) - { - // There is unexpected GPS Group data. - return false; - } - - if (attitudeGroup) - { - if (*reinterpret_cast(curField) != attitudeGroup) - // Not the expected collection of field data types. - return false; - - curField += 2; - } - else if (groups & 0x10) - { - // There is unexpected Attitude Group data. - return false; - } - - if (insGroup) - { - if (*reinterpret_cast(curField) != insGroup) - // Not the expected collection of field data types. - return false; - - curField += 2; - } - else if (groups & 0x20) - { - // There is unexpected INS Group data. - return false; - } - - if(gps2Group) { - if(*reinterpret_cast(curField) != gps2Group) + return false; + + curField += 2; + } else if (groups & 0x01) { + // There is unexpected Common Group data. + return false; + } + + if (timeGroup) { + if (*reinterpret_cast(curField) != timeGroup) // Not the expected collection of field data types. return false; curField += 2; - } else if(groups & 0x40) { - // There is unexpected GPS2 Group data. + } else if (groups & 0x02) { + // There is unexpected Time Group data. return false; } - // Everything checks out. - return true; + if (imuGroup) { + if (*reinterpret_cast(curField) != imuGroup) + // Not the expected collection of field data types. + return false; + curField += 2; + } else if (groups & 0x04) { + // There is unexpected IMU Group data. + return false; + } -} + if (gpsGroup) { + if (*reinterpret_cast(curField) != gpsGroup) + // Not the expected collection of field data types. + return false; -char* startAsciiPacketParse(char* packetStart, size_t& index) -{ - index = 7; + curField += 2; + } else if (groups & 0x08) { + // There is unexpected GPS Group data. + return false; + } + + if (attitudeGroup) { + if (*reinterpret_cast(curField) != attitudeGroup) + // Not the expected collection of field data types. + return false; + + curField += 2; + } else if (groups & 0x10) { + // There is unexpected Attitude Group data. + return false; + } + + if (insGroup) { + if (*reinterpret_cast(curField) != insGroup) + // Not the expected collection of field data types. + return false; + + curField += 2; + } else if (groups & 0x20) { + // There is unexpected INS Group data. + return false; + } + + if (gps2Group) { + if (*reinterpret_cast(curField) != gps2Group) + // Not the expected collection of field data types. + return false; + + curField += 2; + } else if (groups & 0x40) { + // There is unexpected GPS2 Group data. + return false; + } - return vnstrtok(packetStart, index); + // Everything checks out. + return true; } -char* getNextData(char* str, size_t& startIndex) +char * startAsciiPacketParse(char * packetStart, size_t & index) { - return vnstrtok(str, startIndex); + index = 7; + + return vnstrtok(packetStart, index); } -char* vnstrtok(char* str, size_t& startIndex) +char * getNextData(char * str, size_t & startIndex) { return vnstrtok(str, startIndex); } + +char * vnstrtok(char * str, size_t & startIndex) { - size_t origIndex = startIndex; + size_t origIndex = startIndex; - if (str[startIndex-1] == '*') // attempting to read too many fields - return NULL; - - while (str[startIndex] != ',' && str[startIndex] != '*') - { - if((str[startIndex] < ' ') || (str[startIndex] > '~') || (str[startIndex] == '$')) // check for garbage characters - return NULL; - startIndex++; - } - - str[startIndex++] = '\0'; + if (str[startIndex - 1] == '*') // attempting to read too many fields + return NULL; - return str + origIndex; + while (str[startIndex] != ',' && str[startIndex] != '*') { + if ( + (str[startIndex] < ' ') || (str[startIndex] > '~') || + (str[startIndex] == '$')) // check for garbage characters + return NULL; + startIndex++; + } + + str[startIndex++] = '\0'; + + return str + origIndex; } void Packet::ensureCanExtract(size_t numOfBytes) { - if (_curExtractLoc == 0) - // Determine the location to start extracting. - _curExtractLoc = countSetBits(_data[1]) * 2 + 2; + if (_curExtractLoc == 0) + // Determine the location to start extracting. + _curExtractLoc = countSetBits(_data[1]) * 2 + 2; - if (_curExtractLoc + numOfBytes > _length - 2) - // About to overrun data. - throw invalid_operation(); + if (_curExtractLoc + numOfBytes > _length - 2) + // About to overrun data. + throw invalid_operation(); } uint8_t Packet::extractUint8() { - ensureCanExtract(sizeof(uint8_t)); + ensureCanExtract(sizeof(uint8_t)); - uint8_t d = *reinterpret_cast(_data + _curExtractLoc); + uint8_t d = *reinterpret_cast(_data + _curExtractLoc); - _curExtractLoc += sizeof(uint8_t); + _curExtractLoc += sizeof(uint8_t); - return d; + return d; } int8_t Packet::extractInt8() { - ensureCanExtract(sizeof(int8_t)); + ensureCanExtract(sizeof(int8_t)); - int8_t d = *reinterpret_cast(_data + _curExtractLoc); + int8_t d = *reinterpret_cast(_data + _curExtractLoc); - _curExtractLoc += sizeof(int8_t); + _curExtractLoc += sizeof(int8_t); - return d; + return d; } uint16_t Packet::extractUint16() { - ensureCanExtract(sizeof(uint16_t)); + ensureCanExtract(sizeof(uint16_t)); - uint16_t d; + uint16_t d; - memcpy(&d, _data + _curExtractLoc, sizeof(uint16_t)); + memcpy(&d, _data + _curExtractLoc, sizeof(uint16_t)); - _curExtractLoc += sizeof(uint16_t); + _curExtractLoc += sizeof(uint16_t); - return stoh(d); + return stoh(d); } uint32_t Packet::extractUint32() { - ensureCanExtract(sizeof(uint32_t)); + ensureCanExtract(sizeof(uint32_t)); - uint32_t d; + uint32_t d; - memcpy(&d, _data + _curExtractLoc, sizeof(uint32_t)); + memcpy(&d, _data + _curExtractLoc, sizeof(uint32_t)); - _curExtractLoc += sizeof(uint32_t); + _curExtractLoc += sizeof(uint32_t); - return stoh(d); + return stoh(d); } uint64_t Packet::extractUint64() { - ensureCanExtract(sizeof(uint64_t)); + ensureCanExtract(sizeof(uint64_t)); - uint64_t d; + uint64_t d; - memcpy(&d, _data + _curExtractLoc, sizeof(uint64_t)); + memcpy(&d, _data + _curExtractLoc, sizeof(uint64_t)); - _curExtractLoc += sizeof(uint64_t); + _curExtractLoc += sizeof(uint64_t); - return stoh(d); + return stoh(d); } float Packet::extractFloat() { - ensureCanExtract(sizeof(float)); + ensureCanExtract(sizeof(float)); - float f; + float f; - memcpy(&f, _data + _curExtractLoc, sizeof(float)); + memcpy(&f, _data + _curExtractLoc, sizeof(float)); - _curExtractLoc += sizeof(float); + _curExtractLoc += sizeof(float); - return f; + return f; } vec3f Packet::extractVec3f() { - ensureCanExtract(3 * sizeof(float)); + ensureCanExtract(3 * sizeof(float)); - vec3f d; + vec3f d; - memcpy(&d.x, _data + _curExtractLoc, sizeof(float)); - memcpy(&d.y, _data + _curExtractLoc + sizeof(float), sizeof(float)); - memcpy(&d.z, _data + _curExtractLoc + 2 * sizeof(float), sizeof(float)); + memcpy(&d.x, _data + _curExtractLoc, sizeof(float)); + memcpy(&d.y, _data + _curExtractLoc + sizeof(float), sizeof(float)); + memcpy(&d.z, _data + _curExtractLoc + 2 * sizeof(float), sizeof(float)); - _curExtractLoc += 3 * sizeof(float); + _curExtractLoc += 3 * sizeof(float); - return d; + return d; } vec3d Packet::extractVec3d() { - ensureCanExtract(3 * sizeof(double)); + ensureCanExtract(3 * sizeof(double)); - vec3d d; + vec3d d; - memcpy(&d.x, _data + _curExtractLoc, sizeof(double)); - memcpy(&d.y, _data + _curExtractLoc + sizeof(double), sizeof(double)); - memcpy(&d.z, _data + _curExtractLoc + 2 * sizeof(double), sizeof(double)); + memcpy(&d.x, _data + _curExtractLoc, sizeof(double)); + memcpy(&d.y, _data + _curExtractLoc + sizeof(double), sizeof(double)); + memcpy(&d.z, _data + _curExtractLoc + 2 * sizeof(double), sizeof(double)); - _curExtractLoc += 3 * sizeof(double); + _curExtractLoc += 3 * sizeof(double); - return d; + return d; } vec4f Packet::extractVec4f() { - ensureCanExtract(4 * sizeof(float)); + ensureCanExtract(4 * sizeof(float)); - vec4f d; + vec4f d; - memcpy(&d.x, _data + _curExtractLoc, sizeof(float)); - memcpy(&d.y, _data + _curExtractLoc + sizeof(float), sizeof(float)); - memcpy(&d.z, _data + _curExtractLoc + 2 * sizeof(float), sizeof(float)); - memcpy(&d.w, _data + _curExtractLoc + 3 * sizeof(float), sizeof(float)); + memcpy(&d.x, _data + _curExtractLoc, sizeof(float)); + memcpy(&d.y, _data + _curExtractLoc + sizeof(float), sizeof(float)); + memcpy(&d.z, _data + _curExtractLoc + 2 * sizeof(float), sizeof(float)); + memcpy(&d.w, _data + _curExtractLoc + 3 * sizeof(float), sizeof(float)); - _curExtractLoc += 4 * sizeof(float); + _curExtractLoc += 4 * sizeof(float); - return d; + return d; } mat3f Packet::extractMat3f() { - ensureCanExtract(9 * sizeof(float)); + ensureCanExtract(9 * sizeof(float)); - mat3f m; + mat3f m; - memcpy(&m.e00, _data + _curExtractLoc, sizeof(float)); - memcpy(&m.e10, _data + _curExtractLoc + sizeof(float), sizeof(float)); - memcpy(&m.e20, _data + _curExtractLoc + 2 * sizeof(float), sizeof(float)); - memcpy(&m.e01, _data + _curExtractLoc + 3 * sizeof(float), sizeof(float)); - memcpy(&m.e11, _data + _curExtractLoc + 4 * sizeof(float), sizeof(float)); - memcpy(&m.e21, _data + _curExtractLoc + 5 * sizeof(float), sizeof(float)); - memcpy(&m.e02, _data + _curExtractLoc + 6 * sizeof(float), sizeof(float)); - memcpy(&m.e12, _data + _curExtractLoc + 7 * sizeof(float), sizeof(float)); - memcpy(&m.e22, _data + _curExtractLoc + 8 * sizeof(float), sizeof(float)); + memcpy(&m.e00, _data + _curExtractLoc, sizeof(float)); + memcpy(&m.e10, _data + _curExtractLoc + sizeof(float), sizeof(float)); + memcpy(&m.e20, _data + _curExtractLoc + 2 * sizeof(float), sizeof(float)); + memcpy(&m.e01, _data + _curExtractLoc + 3 * sizeof(float), sizeof(float)); + memcpy(&m.e11, _data + _curExtractLoc + 4 * sizeof(float), sizeof(float)); + memcpy(&m.e21, _data + _curExtractLoc + 5 * sizeof(float), sizeof(float)); + memcpy(&m.e02, _data + _curExtractLoc + 6 * sizeof(float), sizeof(float)); + memcpy(&m.e12, _data + _curExtractLoc + 7 * sizeof(float), sizeof(float)); + memcpy(&m.e22, _data + _curExtractLoc + 8 * sizeof(float), sizeof(float)); - _curExtractLoc += 9 * sizeof(float); + _curExtractLoc += 9 * sizeof(float); - return m; -} - -size_t Packet::finalizeCommand(ErrorDetectionMode errorDetectionMode, char *packet, size_t length) -{ - #if defined(_MSC_VER) - // Unable to use save version 'sprintf_s' since the length of 'packet' is unknown here. - #pragma warning(push) - #pragma warning(disable:4996) - #endif - - if (errorDetectionMode == ERRORDETECTIONMODE_CHECKSUM) - { - length += sprintf(packet + length, "*%02X\r\n", Checksum8::compute(packet + 1, length - 1)); - } - else if (errorDetectionMode == ERRORDETECTIONMODE_CRC) - { - length += sprintf(packet + length, "*%04X\r\n", Crc16::compute(packet + 1, length - 1)); - } - else - { - length += sprintf(packet + length, "*XX\r\n"); - } - - #if defined(_MSC_VER) - #pragma warning(pop) - #endif - - return length; -} - -size_t Packet::genReadBinaryOutput1(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) -{ - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,75"); - #else - size_t length = sprintf(buffer, "$VNRRG,75"); - #endif - - return finalizeCommand(errorDetectionMode, buffer, length); -} - -size_t Packet::genReadBinaryOutput2(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) -{ - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,76"); - #else - size_t length = sprintf(buffer, "$VNRRG,76"); - #endif - - return finalizeCommand(errorDetectionMode, buffer, length); + return m; } -size_t Packet::genReadBinaryOutput3(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::finalizeCommand(ErrorDetectionMode errorDetectionMode, char * packet, size_t length) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,77"); - #else - size_t length = sprintf(buffer, "$VNRRG,77"); - #endif +#if defined(_MSC_VER) +// Unable to use save version 'sprintf_s' since the length of 'packet' is unknown here. +#pragma warning(push) +#pragma warning(disable : 4996) +#endif + + if (errorDetectionMode == ERRORDETECTIONMODE_CHECKSUM) { + length += sprintf(packet + length, "*%02X\r\n", Checksum8::compute(packet + 1, length - 1)); + } else if (errorDetectionMode == ERRORDETECTIONMODE_CRC) { + length += sprintf(packet + length, "*%04X\r\n", Crc16::compute(packet + 1, length - 1)); + } else { + length += sprintf(packet + length, "*XX\r\n"); + } + +#if defined(_MSC_VER) +#pragma warning(pop) +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return length; } +size_t Packet::genReadBinaryOutput1( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) +{ +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,75"); +#else + size_t length = sprintf(buffer, "$VNRRG,75"); +#endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} -size_t writeBinaryOutput(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t binaryOutputNumber, uint16_t asyncMode, uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, uint16_t imuField, uint16_t gpsField, uint16_t attitudeField, uint16_t insField, uint16_t gps2Field) +size_t Packet::genReadBinaryOutput2( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - // First determine which groups are present. - uint16_t groups = 0; - if (commonField) - groups |= 0x0001; - if (timeField) - groups |= 0x0002; - if (imuField) - groups |= 0x0004; - if (gpsField) - groups |= 0x0008; - if (attitudeField) - groups |= 0x0010; - if (insField) - groups |= 0x0020; - if(gps2Field) - groups |= 0x0040; +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,76"); +#else + size_t length = sprintf(buffer, "$VNRRG,76"); +#endif - #if VN_HAVE_SECURE_CRT - int length = sprintf_s(buffer, size, "$VNWRG,%u,%u,%u,%X", 74 + binaryOutputNumber, asyncMode, rateDivisor, groups); - #else - int length = sprintf(buffer, "$VNWRG,%u,%u,%u,%X", 74 + binaryOutputNumber, asyncMode, rateDivisor, groups); - #endif + return finalizeCommand(errorDetectionMode, buffer, length); +} - if (commonField) - #if VN_HAVE_SECURE_CRT - length += sprintf_s(buffer + length, size - length, ",%X", commonField); - #else - length += sprintf(buffer + length, ",%X", commonField); - #endif - if (timeField) - #if VN_HAVE_SECURE_CRT - length += sprintf_s(buffer + length, size - length, ",%X", timeField); - #else - length += sprintf(buffer + length, ",%X", timeField); - #endif - if (imuField) - #if VN_HAVE_SECURE_CRT - length += sprintf_s(buffer + length, size - length, ",%X", imuField); - #else - length += sprintf(buffer + length, ",%X", imuField); - #endif - if (gpsField) - #if VN_HAVE_SECURE_CRT - length += sprintf_s(buffer + length, size - length, ",%X", gpsField); - #else - length += sprintf(buffer + length, ",%X", gpsField); - #endif - if (attitudeField) - #if VN_HAVE_SECURE_CRT - length += sprintf_s(buffer + length, size - length, ",%X", attitudeField); - #else - length += sprintf(buffer + length, ",%X", attitudeField); - #endif - if (insField) - #if VN_HAVE_SECURE_CRT - length += sprintf_s(buffer + length, size - length, ",%X", insField); - #else - length += sprintf(buffer + length, ",%X", insField); - #endif - if(gps2Field) - #if VN_HAVE_SECURE_CRT - length += sprintf_s(buffer + length, size - length, ",%X", gps2Field); - #else - length += sprintf(buffer + length, ",%X", gps2Field); - #endif +size_t Packet::genReadBinaryOutput3( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) +{ +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,77"); +#else + size_t length = sprintf(buffer, "$VNRRG,77"); +#endif - return Packet::finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteBinaryOutput1(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint16_t asyncMode, uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, uint16_t imuField, uint16_t gpsField, uint16_t attitudeField, uint16_t insField, uint16_t gps2Field) +size_t writeBinaryOutput( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t binaryOutputNumber, + uint16_t asyncMode, uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, + uint16_t imuField, uint16_t gpsField, uint16_t attitudeField, uint16_t insField, + uint16_t gps2Field) { - return writeBinaryOutput(errorDetectionMode, buffer, size, 1, asyncMode, rateDivisor, commonField, timeField, imuField, gpsField, attitudeField, insField, gps2Field); + // First determine which groups are present. + uint16_t groups = 0; + if (commonField) groups |= 0x0001; + if (timeField) groups |= 0x0002; + if (imuField) groups |= 0x0004; + if (gpsField) groups |= 0x0008; + if (attitudeField) groups |= 0x0010; + if (insField) groups |= 0x0020; + if (gps2Field) groups |= 0x0040; + +#if VN_HAVE_SECURE_CRT + int length = sprintf_s( + buffer, size, "$VNWRG,%u,%u,%u,%X", 74 + binaryOutputNumber, asyncMode, rateDivisor, groups); +#else + int length = + sprintf(buffer, "$VNWRG,%u,%u,%u,%X", 74 + binaryOutputNumber, asyncMode, rateDivisor, groups); +#endif + + if (commonField) +#if VN_HAVE_SECURE_CRT + length += sprintf_s(buffer + length, size - length, ",%X", commonField); +#else + length += sprintf(buffer + length, ",%X", commonField); +#endif + if (timeField) +#if VN_HAVE_SECURE_CRT + length += sprintf_s(buffer + length, size - length, ",%X", timeField); +#else + length += sprintf(buffer + length, ",%X", timeField); +#endif + if (imuField) +#if VN_HAVE_SECURE_CRT + length += sprintf_s(buffer + length, size - length, ",%X", imuField); +#else + length += sprintf(buffer + length, ",%X", imuField); +#endif + if (gpsField) +#if VN_HAVE_SECURE_CRT + length += sprintf_s(buffer + length, size - length, ",%X", gpsField); +#else + length += sprintf(buffer + length, ",%X", gpsField); +#endif + if (attitudeField) +#if VN_HAVE_SECURE_CRT + length += sprintf_s(buffer + length, size - length, ",%X", attitudeField); +#else + length += sprintf(buffer + length, ",%X", attitudeField); +#endif + if (insField) +#if VN_HAVE_SECURE_CRT + length += sprintf_s(buffer + length, size - length, ",%X", insField); +#else + length += sprintf(buffer + length, ",%X", insField); +#endif + if (gps2Field) +#if VN_HAVE_SECURE_CRT + length += sprintf_s(buffer + length, size - length, ",%X", gps2Field); +#else + length += sprintf(buffer + length, ",%X", gps2Field); +#endif + + return Packet::finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteBinaryOutput2(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint16_t asyncMode, uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, uint16_t imuField, uint16_t gpsField, uint16_t attitudeField, uint16_t insField, uint16_t gps2Field) +size_t Packet::genWriteBinaryOutput1( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint16_t asyncMode, + uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, uint16_t imuField, + uint16_t gpsField, uint16_t attitudeField, uint16_t insField, uint16_t gps2Field) { - return writeBinaryOutput(errorDetectionMode, buffer, size, 2, asyncMode, rateDivisor, commonField, timeField, imuField, gpsField, attitudeField, insField, gps2Field); + return writeBinaryOutput( + errorDetectionMode, buffer, size, 1, asyncMode, rateDivisor, commonField, timeField, imuField, + gpsField, attitudeField, insField, gps2Field); } -size_t Packet::genWriteBinaryOutput3(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint16_t asyncMode, uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, uint16_t imuField, uint16_t gpsField, uint16_t attitudeField, uint16_t insField, uint16_t gps2Field) +size_t Packet::genWriteBinaryOutput2( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint16_t asyncMode, + uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, uint16_t imuField, + uint16_t gpsField, uint16_t attitudeField, uint16_t insField, uint16_t gps2Field) { - return writeBinaryOutput(errorDetectionMode, buffer, size, 3, asyncMode, rateDivisor, commonField, timeField, imuField, gpsField, attitudeField, insField, gps2Field); + return writeBinaryOutput( + errorDetectionMode, buffer, size, 2, asyncMode, rateDivisor, commonField, timeField, imuField, + gpsField, attitudeField, insField, gps2Field); } +size_t Packet::genWriteBinaryOutput3( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint16_t asyncMode, + uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, uint16_t imuField, + uint16_t gpsField, uint16_t attitudeField, uint16_t insField, uint16_t gps2Field) +{ + return writeBinaryOutput( + errorDetectionMode, buffer, size, 3, asyncMode, rateDivisor, commonField, timeField, imuField, + gpsField, attitudeField, insField, gps2Field); +} -size_t Packet::genWriteSettings(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genWriteSettings(ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWNV"); - #else - size_t length = sprintf(buffer, "$VNWNV"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWNV"); +#else + size_t length = sprintf(buffer, "$VNWNV"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genTare(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genTare(ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNTAR"); - #else - size_t length = sprintf(buffer, "$VNTAR"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNTAR"); +#else + size_t length = sprintf(buffer, "$VNTAR"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genKnownMagneticDisturbance(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, bool isMagneticDisturbancePresent) +size_t Packet::genKnownMagneticDisturbance( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, + bool isMagneticDisturbancePresent) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNKMD,%d", isMagneticDisturbancePresent ? 1 : 0); - #else - size_t length = sprintf(buffer, "$VNKMD,%d", isMagneticDisturbancePresent ? 1 : 0); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNKMD,%d", isMagneticDisturbancePresent ? 1 : 0); +#else + size_t length = sprintf(buffer, "$VNKMD,%d", isMagneticDisturbancePresent ? 1 : 0); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genKnownAccelerationDisturbance(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, bool isAccelerationDisturbancePresent) +size_t Packet::genKnownAccelerationDisturbance( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, + bool isAccelerationDisturbancePresent) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNKAD,%d", isAccelerationDisturbancePresent ? 1 : 0); - #else - size_t length = sprintf(buffer, "$VNKAD,%d", isAccelerationDisturbancePresent ? 1 : 0); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNKAD,%d", isAccelerationDisturbancePresent ? 1 : 0); +#else + size_t length = sprintf(buffer, "$VNKAD,%d", isAccelerationDisturbancePresent ? 1 : 0); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genSetGyroBias(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genSetGyroBias(ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNSGB"); - #else - size_t length = sprintf(buffer, "$VNSGB"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNSGB"); +#else + size_t length = sprintf(buffer, "$VNSGB"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genRestoreFactorySettings(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genRestoreFactorySettings( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRFS"); - #else - size_t length = sprintf(buffer, "$VNRFS"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRFS"); +#else + size_t length = sprintf(buffer, "$VNRFS"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReset(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReset(ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRST"); - #else - size_t length = sprintf(buffer, "$VNRST"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRST"); +#else + size_t length = sprintf(buffer, "$VNRST"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genFirmwareUpdate(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size) +size_t Packet::genFirmwareUpdate(ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNFWU"); + size_t length = sprintf_s(buffer, size, "$VNFWU"); #else - size_t length = sprintf(buffer, "$VNFWU"); + size_t length = sprintf(buffer, "$VNFWU"); #endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadSerialBaudRate(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t port) +size_t Packet::genReadSerialBaudRate( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t port) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,05,%u", port); - #else - size_t length = sprintf(buffer, "$VNRRG,05,%u", port); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,05,%u", port); +#else + size_t length = sprintf(buffer, "$VNRRG,05,%u", port); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteSerialBaudRate(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint32_t baudrate, uint8_t port) +size_t Packet::genWriteSerialBaudRate( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint32_t baudrate, + uint8_t port) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,05,%u,%u", baudrate, port); - #else - size_t length = sprintf(buffer, "$VNWRG,05,%u,%u", baudrate, port); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,05,%u,%u", baudrate, port); +#else + size_t length = sprintf(buffer, "$VNWRG,05,%u,%u", baudrate, port); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadAsyncDataOutputType(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t port) +size_t Packet::genReadAsyncDataOutputType( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t port) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,06,%u", port); - #else - size_t length = sprintf(buffer, "$VNRRG,06,%u", port); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,06,%u", port); +#else + size_t length = sprintf(buffer, "$VNRRG,06,%u", port); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteAsyncDataOutputType(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint32_t ador, uint8_t port) +size_t Packet::genWriteAsyncDataOutputType( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint32_t ador, uint8_t port) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,06,%u,%u", ador, port); - #else - size_t length = sprintf(buffer, "$VNWRG,06,%u,%u", ador, port); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,06,%u,%u", ador, port); +#else + size_t length = sprintf(buffer, "$VNWRG,06,%u,%u", ador, port); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadAsyncDataOutputFrequency(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t port) +size_t Packet::genReadAsyncDataOutputFrequency( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t port) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,07,%u", port); - #else - size_t length = sprintf(buffer, "$VNRRG,07,%u", port); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,07,%u", port); +#else + size_t length = sprintf(buffer, "$VNRRG,07,%u", port); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteAsyncDataOutputFrequency(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint32_t adof, uint8_t port) +size_t Packet::genWriteAsyncDataOutputFrequency( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint32_t adof, uint8_t port) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,07,%u,%u", adof, port); - #else - size_t length = sprintf(buffer, "$VNWRG,07,%u,%u", adof, port); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,07,%u,%u", adof, port); +#else + size_t length = sprintf(buffer, "$VNWRG,07,%u,%u", adof, port); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteFilterMeasurementsVarianceParameters(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, float angularWalkVariance, vec3f angularRateVariance, vec3f magneticVariance, vec3f accelerationVariance) +size_t Packet::genWriteFilterMeasurementsVarianceParameters( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, float angularWalkVariance, + vec3f angularRateVariance, vec3f magneticVariance, vec3f accelerationVariance) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,22,%E,%E,%E,%E,%f,%f,%f,%f,%f,%f", - #else - size_t length = sprintf(buffer, "$VNWRG,22,%E,%E,%E,%E,%f,%f,%f,%f,%f,%f", - #endif - angularWalkVariance, - angularRateVariance.x, - angularRateVariance.y, - angularRateVariance.z, - magneticVariance.x, - magneticVariance.y, - magneticVariance.z, - accelerationVariance.x, - accelerationVariance.y, - accelerationVariance.z); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,22,%E,%E,%E,%E,%f,%f,%f,%f,%f,%f", +#else + size_t length = sprintf( + buffer, "$VNWRG,22,%E,%E,%E,%E,%f,%f,%f,%f,%f,%f", +#endif + angularWalkVariance, angularRateVariance.x, angularRateVariance.y, angularRateVariance.z, + magneticVariance.x, magneticVariance.y, magneticVariance.z, accelerationVariance.x, + accelerationVariance.y, accelerationVariance.z); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteFirmwareUpdateRecord(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, string record) +size_t Packet::genWriteFirmwareUpdateRecord( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, string record) { #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNBLD,%s" + size_t length = sprintf_s( + buffer, size, "$VNBLD,%s" #else - size_t length = sprintf(buffer, "$VNBLD,%s" + size_t length = sprintf( + buffer, "$VNBLD,%s" #endif - , - record.c_str()); + , + record.c_str()); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadUserTag(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadUserTag(ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,00"); - #else - size_t length = sprintf(buffer, "$VNRRG,00"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,00"); +#else + size_t length = sprintf(buffer, "$VNRRG,00"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteUserTag(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, string tag) +size_t Packet::genWriteUserTag( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, string tag) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,00,%s" - #else - size_t length = sprintf(buffer, "$VNWRG,00,%s" - #endif -, - tag.c_str()); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,00,%s" +#else + size_t length = sprintf( + buffer, "$VNWRG,00,%s" +#endif + , + tag.c_str()); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadModelNumber(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadModelNumber(ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,01"); - #else - size_t length = sprintf(buffer, "$VNRRG,01"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,01"); +#else + size_t length = sprintf(buffer, "$VNRRG,01"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadHardwareRevision(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadHardwareRevision( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,02"); - #else - size_t length = sprintf(buffer, "$VNRRG,02"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,02"); +#else + size_t length = sprintf(buffer, "$VNRRG,02"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadSerialNumber(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadSerialNumber( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,03"); - #else - size_t length = sprintf(buffer, "$VNRRG,03"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,03"); +#else + size_t length = sprintf(buffer, "$VNRRG,03"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadFirmwareVersion(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadFirmwareVersion( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,04"); - #else - size_t length = sprintf(buffer, "$VNRRG,04"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,04"); +#else + size_t length = sprintf(buffer, "$VNRRG,04"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadSerialBaudRate(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadSerialBaudRate( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,05"); - #else - size_t length = sprintf(buffer, "$VNRRG,05"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,05"); +#else + size_t length = sprintf(buffer, "$VNRRG,05"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteSerialBaudRate(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint32_t baudrate) +size_t Packet::genWriteSerialBaudRate( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint32_t baudrate) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,05,%u" - #else - size_t length = sprintf(buffer, "$VNWRG,05,%u" - #endif -, - baudrate); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,05,%u" +#else + size_t length = sprintf( + buffer, "$VNWRG,05,%u" +#endif + , + baudrate); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadAsyncDataOutputType(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadAsyncDataOutputType( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,06"); - #else - size_t length = sprintf(buffer, "$VNRRG,06"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,06"); +#else + size_t length = sprintf(buffer, "$VNRRG,06"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteAsyncDataOutputType(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint32_t ador) +size_t Packet::genWriteAsyncDataOutputType( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint32_t ador) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,06,%u" - #else - size_t length = sprintf(buffer, "$VNWRG,06,%u" - #endif -, - ador); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,06,%u" +#else + size_t length = sprintf( + buffer, "$VNWRG,06,%u" +#endif + , + ador); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadAsyncDataOutputFrequency(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadAsyncDataOutputFrequency( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,07"); - #else - size_t length = sprintf(buffer, "$VNRRG,07"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,07"); +#else + size_t length = sprintf(buffer, "$VNRRG,07"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteAsyncDataOutputFrequency(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint32_t adof) +size_t Packet::genWriteAsyncDataOutputFrequency( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint32_t adof) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,07,%u" - #else - size_t length = sprintf(buffer, "$VNWRG,07,%u" - #endif -, - adof); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,07,%u" +#else + size_t length = sprintf( + buffer, "$VNWRG,07,%u" +#endif + , + adof); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadYawPitchRoll(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadYawPitchRoll( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,08"); - #else - size_t length = sprintf(buffer, "$VNRRG,08"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,08"); +#else + size_t length = sprintf(buffer, "$VNRRG,08"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadAttitudeQuaternion(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadAttitudeQuaternion( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,09"); - #else - size_t length = sprintf(buffer, "$VNRRG,09"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,09"); +#else + size_t length = sprintf(buffer, "$VNRRG,09"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadQuaternionMagneticAccelerationAndAngularRates(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadQuaternionMagneticAccelerationAndAngularRates( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,15"); - #else - size_t length = sprintf(buffer, "$VNRRG,15"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,15"); +#else + size_t length = sprintf(buffer, "$VNRRG,15"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadMagneticMeasurements(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadMagneticMeasurements( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,17"); - #else - size_t length = sprintf(buffer, "$VNRRG,17"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,17"); +#else + size_t length = sprintf(buffer, "$VNRRG,17"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadAccelerationMeasurements(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadAccelerationMeasurements( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,18"); - #else - size_t length = sprintf(buffer, "$VNRRG,18"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,18"); +#else + size_t length = sprintf(buffer, "$VNRRG,18"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadAngularRateMeasurements(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadAngularRateMeasurements( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,19"); - #else - size_t length = sprintf(buffer, "$VNRRG,19"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,19"); +#else + size_t length = sprintf(buffer, "$VNRRG,19"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadMagneticAccelerationAndAngularRates(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadMagneticAccelerationAndAngularRates( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,20"); - #else - size_t length = sprintf(buffer, "$VNRRG,20"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,20"); +#else + size_t length = sprintf(buffer, "$VNRRG,20"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadMagneticAndGravityReferenceVectors(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadMagneticAndGravityReferenceVectors( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,21"); - #else - size_t length = sprintf(buffer, "$VNRRG,21"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,21"); +#else + size_t length = sprintf(buffer, "$VNRRG,21"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteMagneticAndGravityReferenceVectors(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f magRef, vec3f accRef) +size_t Packet::genWriteMagneticAndGravityReferenceVectors( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vec3f magRef, vec3f accRef) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,21,%f,%f,%f,%f,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,21,%f,%f,%f,%f,%f,%f" - #endif -, - magRef.x, - magRef.y, - magRef.z, - accRef.x, - accRef.y, - accRef.z); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,21,%f,%f,%f,%f,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,21,%f,%f,%f,%f,%f,%f" +#endif + , + magRef.x, magRef.y, magRef.z, accRef.x, accRef.y, accRef.z); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadFilterMeasurementsVarianceParameters(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadFilterMeasurementsVarianceParameters( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,22"); - #else - size_t length = sprintf(buffer, "$VNRRG,22"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,22"); +#else + size_t length = sprintf(buffer, "$VNRRG,22"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadMagnetometerCompensation(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadMagnetometerCompensation( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,23"); - #else - size_t length = sprintf(buffer, "$VNRRG,23"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,23"); +#else + size_t length = sprintf(buffer, "$VNRRG,23"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteMagnetometerCompensation(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, mat3f c, vec3f b) +size_t Packet::genWriteMagnetometerCompensation( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, mat3f c, vec3f b) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,23,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,23,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f" - #endif -, - c.e00, - c.e01, - c.e02, - c.e10, - c.e11, - c.e12, - c.e20, - c.e21, - c.e22, - b.x, - b.y, - b.z); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,23,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,23,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f" +#endif + , + c.e00, c.e01, c.e02, c.e10, c.e11, c.e12, c.e20, c.e21, c.e22, b.x, b.y, b.z); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadFilterActiveTuningParameters(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadFilterActiveTuningParameters( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,24"); - #else - size_t length = sprintf(buffer, "$VNRRG,24"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,24"); +#else + size_t length = sprintf(buffer, "$VNRRG,24"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteFilterActiveTuningParameters(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, float magneticDisturbanceGain, float accelerationDisturbanceGain, float magneticDisturbanceMemory, float accelerationDisturbanceMemory) +size_t Packet::genWriteFilterActiveTuningParameters( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, float magneticDisturbanceGain, + float accelerationDisturbanceGain, float magneticDisturbanceMemory, + float accelerationDisturbanceMemory) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,24,%f,%f,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,24,%f,%f,%f,%f" - #endif -, - magneticDisturbanceGain, - accelerationDisturbanceGain, - magneticDisturbanceMemory, - accelerationDisturbanceMemory); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,24,%f,%f,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,24,%f,%f,%f,%f" +#endif + , + magneticDisturbanceGain, accelerationDisturbanceGain, magneticDisturbanceMemory, + accelerationDisturbanceMemory); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadAccelerationCompensation(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadAccelerationCompensation( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,25"); - #else - size_t length = sprintf(buffer, "$VNRRG,25"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,25"); +#else + size_t length = sprintf(buffer, "$VNRRG,25"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteAccelerationCompensation(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, mat3f c, vec3f b) +size_t Packet::genWriteAccelerationCompensation( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, mat3f c, vec3f b) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,25,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,25,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f" - #endif -, - c.e00, - c.e01, - c.e02, - c.e10, - c.e11, - c.e12, - c.e20, - c.e21, - c.e22, - b.x, - b.y, - b.z); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,25,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,25,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f" +#endif + , + c.e00, c.e01, c.e02, c.e10, c.e11, c.e12, c.e20, c.e21, c.e22, b.x, b.y, b.z); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadReferenceFrameRotation(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadReferenceFrameRotation( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,26"); - #else - size_t length = sprintf(buffer, "$VNRRG,26"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,26"); +#else + size_t length = sprintf(buffer, "$VNRRG,26"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteReferenceFrameRotation(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, mat3f c) +size_t Packet::genWriteReferenceFrameRotation( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, mat3f c) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,26,%f,%f,%f,%f,%f,%f,%f,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,26,%f,%f,%f,%f,%f,%f,%f,%f,%f" - #endif -, - c.e00, - c.e01, - c.e02, - c.e10, - c.e11, - c.e12, - c.e20, - c.e21, - c.e22); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,26,%f,%f,%f,%f,%f,%f,%f,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,26,%f,%f,%f,%f,%f,%f,%f,%f,%f" +#endif + , + c.e00, c.e01, c.e02, c.e10, c.e11, c.e12, c.e20, c.e21, c.e22); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadYawPitchRollMagneticAccelerationAndAngularRates(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadYawPitchRollMagneticAccelerationAndAngularRates( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,27"); - #else - size_t length = sprintf(buffer, "$VNRRG,27"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,27"); +#else + size_t length = sprintf(buffer, "$VNRRG,27"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadCommunicationProtocolControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadCommunicationProtocolControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,30"); - #else - size_t length = sprintf(buffer, "$VNRRG,30"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,30"); +#else + size_t length = sprintf(buffer, "$VNRRG,30"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteCommunicationProtocolControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t serialCount, uint8_t serialStatus, uint8_t spiCount, uint8_t spiStatus, uint8_t serialChecksum, uint8_t spiChecksum, uint8_t errorMode) +size_t Packet::genWriteCommunicationProtocolControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t serialCount, + uint8_t serialStatus, uint8_t spiCount, uint8_t spiStatus, uint8_t serialChecksum, + uint8_t spiChecksum, uint8_t errorMode) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,30,%u,%u,%u,%u,%u,%u,%u" - #else - size_t length = sprintf(buffer, "$VNWRG,30,%u,%u,%u,%u,%u,%u,%u" - #endif -, - serialCount, - serialStatus, - spiCount, - spiStatus, - serialChecksum, - spiChecksum, - errorMode); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,30,%u,%u,%u,%u,%u,%u,%u" +#else + size_t length = sprintf( + buffer, "$VNWRG,30,%u,%u,%u,%u,%u,%u,%u" +#endif + , + serialCount, serialStatus, spiCount, spiStatus, serialChecksum, spiChecksum, errorMode); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadSynchronizationControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadSynchronizationControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,32"); - #else - size_t length = sprintf(buffer, "$VNRRG,32"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,32"); +#else + size_t length = sprintf(buffer, "$VNRRG,32"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteSynchronizationControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t syncInMode, uint8_t syncInEdge, uint16_t syncInSkipFactor, uint8_t syncOutMode, uint8_t syncOutPolarity, uint16_t syncOutSkipFactor, uint32_t syncOutPulseWidth) +size_t Packet::genWriteSynchronizationControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t syncInMode, + uint8_t syncInEdge, uint16_t syncInSkipFactor, uint8_t syncOutMode, uint8_t syncOutPolarity, + uint16_t syncOutSkipFactor, uint32_t syncOutPulseWidth) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,32,%u,%u,%u,0,%u,%u,%u,%u,0" - #else - size_t length = sprintf(buffer, "$VNWRG,32,%u,%u,%u,0,%u,%u,%u,%u,0" - #endif -, - syncInMode, - syncInEdge, - syncInSkipFactor, - syncOutMode, - syncOutPolarity, - syncOutSkipFactor, - syncOutPulseWidth); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,32,%u,%u,%u,0,%u,%u,%u,%u,0" +#else + size_t length = sprintf( + buffer, "$VNWRG,32,%u,%u,%u,0,%u,%u,%u,%u,0" +#endif + , + syncInMode, syncInEdge, syncInSkipFactor, syncOutMode, syncOutPolarity, syncOutSkipFactor, + syncOutPulseWidth); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadSynchronizationStatus(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadSynchronizationStatus( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,33"); - #else - size_t length = sprintf(buffer, "$VNRRG,33"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,33"); +#else + size_t length = sprintf(buffer, "$VNRRG,33"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteSynchronizationStatus(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint32_t syncInCount, uint32_t syncInTime, uint32_t syncOutCount) +size_t Packet::genWriteSynchronizationStatus( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint32_t syncInCount, + uint32_t syncInTime, uint32_t syncOutCount) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,33,%u,%u,%u" - #else - size_t length = sprintf(buffer, "$VNWRG,33,%u,%u,%u" - #endif -, - syncInCount, - syncInTime, - syncOutCount); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,33,%u,%u,%u" +#else + size_t length = sprintf( + buffer, "$VNWRG,33,%u,%u,%u" +#endif + , + syncInCount, syncInTime, syncOutCount); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadFilterBasicControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadFilterBasicControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,34"); - #else - size_t length = sprintf(buffer, "$VNRRG,34"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,34"); +#else + size_t length = sprintf(buffer, "$VNRRG,34"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteFilterBasicControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t magMode, uint8_t extMagMode, uint8_t extAccMode, uint8_t extGyroMode, vec3f gyroLimit) +size_t Packet::genWriteFilterBasicControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t magMode, + uint8_t extMagMode, uint8_t extAccMode, uint8_t extGyroMode, vec3f gyroLimit) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,34,%u,%u,%u,%u,%f,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,34,%u,%u,%u,%u,%f,%f,%f" - #endif -, - magMode, - extMagMode, - extAccMode, - extGyroMode, - gyroLimit.x, - gyroLimit.y, - gyroLimit.z); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,34,%u,%u,%u,%u,%f,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,34,%u,%u,%u,%u,%f,%f,%f" +#endif + , + magMode, extMagMode, extAccMode, extGyroMode, gyroLimit.x, gyroLimit.y, gyroLimit.z); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadHeaveConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadHeaveConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,116"); - #else - size_t length = sprintf(buffer, "$VNRRG,116"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,116"); +#else + size_t length = sprintf(buffer, "$VNRRG,116"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteHeaveConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, - float initialWavePeriod, - float initialWaveAmplitude, - float maxWavePeriod, - float minWaveAmplitude, - float delayedHeaveCutoffFreq, - float heaveCutoffFreq, - float heaveRateCutoffFreq) +size_t Packet::genWriteHeaveConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, float initialWavePeriod, + float initialWaveAmplitude, float maxWavePeriod, float minWaveAmplitude, + float delayedHeaveCutoffFreq, float heaveCutoffFreq, float heaveRateCutoffFreq) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,116,%f,%f,%f,%f,%f,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,116,%f,%f,%f,%f,%f,%f,%f" - #endif -, - initialWavePeriod, - initialWaveAmplitude, - maxWavePeriod, - minWaveAmplitude, - delayedHeaveCutoffFreq, - heaveCutoffFreq, - heaveRateCutoffFreq); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,116,%f,%f,%f,%f,%f,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,116,%f,%f,%f,%f,%f,%f,%f" +#endif + , + initialWavePeriod, initialWaveAmplitude, maxWavePeriod, minWaveAmplitude, + delayedHeaveCutoffFreq, heaveCutoffFreq, heaveRateCutoffFreq); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadVpeBasicControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadVpeBasicControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,35"); - #else - size_t length = sprintf(buffer, "$VNRRG,35"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,35"); +#else + size_t length = sprintf(buffer, "$VNRRG,35"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteVpeBasicControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t enable, uint8_t headingMode, uint8_t filteringMode, uint8_t tuningMode) +size_t Packet::genWriteVpeBasicControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t enable, + uint8_t headingMode, uint8_t filteringMode, uint8_t tuningMode) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,35,%u,%u,%u,%u" - #else - size_t length = sprintf(buffer, "$VNWRG,35,%u,%u,%u,%u" - #endif -, - enable, - headingMode, - filteringMode, - tuningMode); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,35,%u,%u,%u,%u" +#else + size_t length = sprintf( + buffer, "$VNWRG,35,%u,%u,%u,%u" +#endif + , + enable, headingMode, filteringMode, tuningMode); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadVpeMagnetometerBasicTuning(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadVpeMagnetometerBasicTuning( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,36"); - #else - size_t length = sprintf(buffer, "$VNRRG,36"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,36"); +#else + size_t length = sprintf(buffer, "$VNRRG,36"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteVpeMagnetometerBasicTuning(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f baseTuning, vec3f adaptiveTuning, vec3f adaptiveFiltering) +size_t Packet::genWriteVpeMagnetometerBasicTuning( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vec3f baseTuning, + vec3f adaptiveTuning, vec3f adaptiveFiltering) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,36,%f,%f,%f,%f,%f,%f,%f,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,36,%f,%f,%f,%f,%f,%f,%f,%f,%f" - #endif -, - baseTuning.x, - baseTuning.y, - baseTuning.z, - adaptiveTuning.x, - adaptiveTuning.y, - adaptiveTuning.z, - adaptiveFiltering.x, - adaptiveFiltering.y, - adaptiveFiltering.z); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,36,%f,%f,%f,%f,%f,%f,%f,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,36,%f,%f,%f,%f,%f,%f,%f,%f,%f" +#endif + , + baseTuning.x, baseTuning.y, baseTuning.z, adaptiveTuning.x, adaptiveTuning.y, adaptiveTuning.z, + adaptiveFiltering.x, adaptiveFiltering.y, adaptiveFiltering.z); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadVpeMagnetometerAdvancedTuning(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadVpeMagnetometerAdvancedTuning( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,37"); - #else - size_t length = sprintf(buffer, "$VNRRG,37"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,37"); +#else + size_t length = sprintf(buffer, "$VNRRG,37"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteVpeMagnetometerAdvancedTuning(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f minFiltering, vec3f maxFiltering, float maxAdaptRate, float disturbanceWindow, float maxTuning) +size_t Packet::genWriteVpeMagnetometerAdvancedTuning( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vec3f minFiltering, + vec3f maxFiltering, float maxAdaptRate, float disturbanceWindow, float maxTuning) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,37,%f,%f,%f,%f,%f,%f,%f,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,37,%f,%f,%f,%f,%f,%f,%f,%f,%f" - #endif -, - minFiltering.x, - minFiltering.y, - minFiltering.z, - maxFiltering.x, - maxFiltering.y, - maxFiltering.z, - maxAdaptRate, - disturbanceWindow, - maxTuning); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,37,%f,%f,%f,%f,%f,%f,%f,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,37,%f,%f,%f,%f,%f,%f,%f,%f,%f" +#endif + , + minFiltering.x, minFiltering.y, minFiltering.z, maxFiltering.x, maxFiltering.y, maxFiltering.z, + maxAdaptRate, disturbanceWindow, maxTuning); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadVpeAccelerometerBasicTuning(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadVpeAccelerometerBasicTuning( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,38"); - #else - size_t length = sprintf(buffer, "$VNRRG,38"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,38"); +#else + size_t length = sprintf(buffer, "$VNRRG,38"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteVpeAccelerometerBasicTuning(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f baseTuning, vec3f adaptiveTuning, vec3f adaptiveFiltering) +size_t Packet::genWriteVpeAccelerometerBasicTuning( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vec3f baseTuning, + vec3f adaptiveTuning, vec3f adaptiveFiltering) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,38,%f,%f,%f,%f,%f,%f,%f,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,38,%f,%f,%f,%f,%f,%f,%f,%f,%f" - #endif -, - baseTuning.x, - baseTuning.y, - baseTuning.z, - adaptiveTuning.x, - adaptiveTuning.y, - adaptiveTuning.z, - adaptiveFiltering.x, - adaptiveFiltering.y, - adaptiveFiltering.z); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,38,%f,%f,%f,%f,%f,%f,%f,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,38,%f,%f,%f,%f,%f,%f,%f,%f,%f" +#endif + , + baseTuning.x, baseTuning.y, baseTuning.z, adaptiveTuning.x, adaptiveTuning.y, adaptiveTuning.z, + adaptiveFiltering.x, adaptiveFiltering.y, adaptiveFiltering.z); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadVpeAccelerometerAdvancedTuning(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadVpeAccelerometerAdvancedTuning( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,39"); - #else - size_t length = sprintf(buffer, "$VNRRG,39"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,39"); +#else + size_t length = sprintf(buffer, "$VNRRG,39"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteVpeAccelerometerAdvancedTuning(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f minFiltering, vec3f maxFiltering, float maxAdaptRate, float disturbanceWindow, float maxTuning) +size_t Packet::genWriteVpeAccelerometerAdvancedTuning( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vec3f minFiltering, + vec3f maxFiltering, float maxAdaptRate, float disturbanceWindow, float maxTuning) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,39,%f,%f,%f,%f,%f,%f,%f,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,39,%f,%f,%f,%f,%f,%f,%f,%f,%f" - #endif -, - minFiltering.x, - minFiltering.y, - minFiltering.z, - maxFiltering.x, - maxFiltering.y, - maxFiltering.z, - maxAdaptRate, - disturbanceWindow, - maxTuning); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,39,%f,%f,%f,%f,%f,%f,%f,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,39,%f,%f,%f,%f,%f,%f,%f,%f,%f" +#endif + , + minFiltering.x, minFiltering.y, minFiltering.z, maxFiltering.x, maxFiltering.y, maxFiltering.z, + maxAdaptRate, disturbanceWindow, maxTuning); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadVpeGyroBasicTuning(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadVpeGyroBasicTuning( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,40"); - #else - size_t length = sprintf(buffer, "$VNRRG,40"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,40"); +#else + size_t length = sprintf(buffer, "$VNRRG,40"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteVpeGyroBasicTuning(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f angularWalkVariance, vec3f baseTuning, vec3f adaptiveTuning) +size_t Packet::genWriteVpeGyroBasicTuning( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vec3f angularWalkVariance, + vec3f baseTuning, vec3f adaptiveTuning) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,40,%f,%f,%f,%f,%f,%f,%f,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,40,%f,%f,%f,%f,%f,%f,%f,%f,%f" - #endif -, - angularWalkVariance.x, - angularWalkVariance.y, - angularWalkVariance.z, - baseTuning.x, - baseTuning.y, - baseTuning.z, - adaptiveTuning.x, - adaptiveTuning.y, - adaptiveTuning.z); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,40,%f,%f,%f,%f,%f,%f,%f,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,40,%f,%f,%f,%f,%f,%f,%f,%f,%f" +#endif + , + angularWalkVariance.x, angularWalkVariance.y, angularWalkVariance.z, baseTuning.x, baseTuning.y, + baseTuning.z, adaptiveTuning.x, adaptiveTuning.y, adaptiveTuning.z); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadFilterStartupGyroBias(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadFilterStartupGyroBias( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,43"); - #else - size_t length = sprintf(buffer, "$VNRRG,43"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,43"); +#else + size_t length = sprintf(buffer, "$VNRRG,43"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteFilterStartupGyroBias(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f bias) +size_t Packet::genWriteFilterStartupGyroBias( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vec3f bias) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,43,%f,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,43,%f,%f,%f" - #endif -, - bias.x, - bias.y, - bias.z); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,43,%f,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,43,%f,%f,%f" +#endif + , + bias.x, bias.y, bias.z); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadMagnetometerCalibrationControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadMagnetometerCalibrationControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,44"); - #else - size_t length = sprintf(buffer, "$VNRRG,44"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,44"); +#else + size_t length = sprintf(buffer, "$VNRRG,44"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteMagnetometerCalibrationControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t hsiMode, uint8_t hsiOutput, uint8_t convergeRate) +size_t Packet::genWriteMagnetometerCalibrationControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t hsiMode, + uint8_t hsiOutput, uint8_t convergeRate) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,44,%u,%u,%u" - #else - size_t length = sprintf(buffer, "$VNWRG,44,%u,%u,%u" - #endif -, - hsiMode, - hsiOutput, - convergeRate); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,44,%u,%u,%u" +#else + size_t length = sprintf( + buffer, "$VNWRG,44,%u,%u,%u" +#endif + , + hsiMode, hsiOutput, convergeRate); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadCalculatedMagnetometerCalibration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadCalculatedMagnetometerCalibration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,47"); - #else - size_t length = sprintf(buffer, "$VNRRG,47"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,47"); +#else + size_t length = sprintf(buffer, "$VNRRG,47"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadIndoorHeadingModeControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadIndoorHeadingModeControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,48"); - #else - size_t length = sprintf(buffer, "$VNRRG,48"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,48"); +#else + size_t length = sprintf(buffer, "$VNRRG,48"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteIndoorHeadingModeControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, float maxRateError) +size_t Packet::genWriteIndoorHeadingModeControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, float maxRateError) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,48,%f,0" - #else - size_t length = sprintf(buffer, "$VNWRG,48,%f,0" - #endif -, - maxRateError); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,48,%f,0" +#else + size_t length = sprintf( + buffer, "$VNWRG,48,%f,0" +#endif + , + maxRateError); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadVelocityCompensationMeasurement(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadVelocityCompensationMeasurement( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,50"); - #else - size_t length = sprintf(buffer, "$VNRRG,50"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,50"); +#else + size_t length = sprintf(buffer, "$VNRRG,50"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteVelocityCompensationMeasurement(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f velocity) +size_t Packet::genWriteVelocityCompensationMeasurement( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vec3f velocity) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,50,%f,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,50,%f,%f,%f" - #endif -, - velocity.x, - velocity.y, - velocity.z); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,50,%f,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,50,%f,%f,%f" +#endif + , + velocity.x, velocity.y, velocity.z); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadVelocityCompensationControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadVelocityCompensationControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,51"); - #else - size_t length = sprintf(buffer, "$VNRRG,51"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,51"); +#else + size_t length = sprintf(buffer, "$VNRRG,51"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteVelocityCompensationControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t mode, float velocityTuning, float rateTuning) +size_t Packet::genWriteVelocityCompensationControl( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t mode, + float velocityTuning, float rateTuning) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,51,%u,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,51,%u,%f,%f" - #endif -, - mode, - velocityTuning, - rateTuning); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,51,%u,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,51,%u,%f,%f" +#endif + , + mode, velocityTuning, rateTuning); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadVelocityCompensationStatus(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadVelocityCompensationStatus( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,52"); - #else - size_t length = sprintf(buffer, "$VNRRG,52"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,52"); +#else + size_t length = sprintf(buffer, "$VNRRG,52"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadImuMeasurements(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadImuMeasurements( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,54"); - #else - size_t length = sprintf(buffer, "$VNRRG,54"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,54"); +#else + size_t length = sprintf(buffer, "$VNRRG,54"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadGpsConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadGpsConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,55"); - #else - size_t length = sprintf(buffer, "$VNRRG,55"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,55"); +#else + size_t length = sprintf(buffer, "$VNRRG,55"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteGpsConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t mode, uint8_t ppsSource) +size_t Packet::genWriteGpsConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t mode, + uint8_t ppsSource) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,55,%u,%u,5,0,0" - #else - size_t length = sprintf(buffer, "$VNWRG,55,%u,%u,5,0,0" - #endif -, - mode, - ppsSource); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,55,%u,%u,5,0,0" +#else + size_t length = sprintf( + buffer, "$VNWRG,55,%u,%u,5,0,0" +#endif + , + mode, ppsSource); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteGpsConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t mode, uint8_t ppsSource, uint8_t rate, uint8_t antPow) +size_t Packet::genWriteGpsConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t mode, + uint8_t ppsSource, uint8_t rate, uint8_t antPow) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,55,%u,%u,%u,0,%u" - #else - size_t length = sprintf(buffer, "$VNWRG,55,%u,%u,%u,0,%u" - #endif -, - mode, - ppsSource, - rate, - antPow); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,55,%u,%u,%u,0,%u" +#else + size_t length = sprintf( + buffer, "$VNWRG,55,%u,%u,%u,0,%u" +#endif + , + mode, ppsSource, rate, antPow); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadGpsAntennaOffset(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadGpsAntennaOffset( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,57"); - #else - size_t length = sprintf(buffer, "$VNRRG,57"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,57"); +#else + size_t length = sprintf(buffer, "$VNRRG,57"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteGpsAntennaOffset(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f position) +size_t Packet::genWriteGpsAntennaOffset( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vec3f position) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,57,%f,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,57,%f,%f,%f" - #endif -, - position.x, - position.y, - position.z); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,57,%f,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,57,%f,%f,%f" +#endif + , + position.x, position.y, position.z); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadGpsSolutionLla(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadGpsSolutionLla( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,58"); - #else - size_t length = sprintf(buffer, "$VNRRG,58"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,58"); +#else + size_t length = sprintf(buffer, "$VNRRG,58"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadGpsSolutionEcef(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadGpsSolutionEcef( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,59"); - #else - size_t length = sprintf(buffer, "$VNRRG,59"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,59"); +#else + size_t length = sprintf(buffer, "$VNRRG,59"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadInsSolutionLla(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadInsSolutionLla( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,63"); - #else - size_t length = sprintf(buffer, "$VNRRG,63"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,63"); +#else + size_t length = sprintf(buffer, "$VNRRG,63"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadInsSolutionEcef(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadInsSolutionEcef( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,64"); - #else - size_t length = sprintf(buffer, "$VNRRG,64"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,64"); +#else + size_t length = sprintf(buffer, "$VNRRG,64"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadInsBasicConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadInsBasicConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,67"); - #else - size_t length = sprintf(buffer, "$VNRRG,67"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,67"); +#else + size_t length = sprintf(buffer, "$VNRRG,67"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteInsBasicConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t scenario, uint8_t ahrsAiding, uint8_t estBaseline) +size_t Packet::genWriteInsBasicConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t scenario, + uint8_t ahrsAiding, uint8_t estBaseline) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,67,%u,%u,%u,0" - #else - size_t length = sprintf(buffer, "$VNWRG,67,%u,%u,%u,0" - #endif -, - scenario, - ahrsAiding, - estBaseline); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,67,%u,%u,%u,0" +#else + size_t length = sprintf( + buffer, "$VNWRG,67,%u,%u,%u,0" +#endif + , + scenario, ahrsAiding, estBaseline); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadInsAdvancedConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadInsAdvancedConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,68"); - #else - size_t length = sprintf(buffer, "$VNRRG,68"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,68"); +#else + size_t length = sprintf(buffer, "$VNRRG,68"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteInsAdvancedConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t useMag, uint8_t usePres, uint8_t posAtt, uint8_t velAtt, uint8_t velBias, uint8_t useFoam, uint8_t gpsCovType, uint8_t velCount, float velInit, float moveOrigin, float gpsTimeout, float deltaLimitPos, float deltaLimitVel, float minPosUncertainty, float minVelUncertainty) +size_t Packet::genWriteInsAdvancedConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t useMag, + uint8_t usePres, uint8_t posAtt, uint8_t velAtt, uint8_t velBias, uint8_t useFoam, + uint8_t gpsCovType, uint8_t velCount, float velInit, float moveOrigin, float gpsTimeout, + float deltaLimitPos, float deltaLimitVel, float minPosUncertainty, float minVelUncertainty) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,68,%u,%u,%u,%u,%u,%u,%u,%u,%f,%f,%f,%f,%f,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,68,%u,%u,%u,%u,%u,%u,%u,%u,%f,%f,%f,%f,%f,%f,%f" - #endif -, - useMag, - usePres, - posAtt, - velAtt, - velBias, - useFoam, - gpsCovType, - velCount, - velInit, - moveOrigin, - gpsTimeout, - deltaLimitPos, - deltaLimitVel, - minPosUncertainty, - minVelUncertainty); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,68,%u,%u,%u,%u,%u,%u,%u,%u,%f,%f,%f,%f,%f,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,68,%u,%u,%u,%u,%u,%u,%u,%u,%f,%f,%f,%f,%f,%f,%f" +#endif + , + useMag, usePres, posAtt, velAtt, velBias, useFoam, gpsCovType, velCount, velInit, moveOrigin, + gpsTimeout, deltaLimitPos, deltaLimitVel, minPosUncertainty, minVelUncertainty); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadInsStateLla(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadInsStateLla(ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,72"); - #else - size_t length = sprintf(buffer, "$VNRRG,72"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,72"); +#else + size_t length = sprintf(buffer, "$VNRRG,72"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadInsStateEcef(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadInsStateEcef( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,73"); - #else - size_t length = sprintf(buffer, "$VNRRG,73"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,73"); +#else + size_t length = sprintf(buffer, "$VNRRG,73"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadStartupFilterBiasEstimate(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadStartupFilterBiasEstimate( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,74"); - #else - size_t length = sprintf(buffer, "$VNRRG,74"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,74"); +#else + size_t length = sprintf(buffer, "$VNRRG,74"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteStartupFilterBiasEstimate(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f gyroBias, vec3f accelBias, float pressureBias) +size_t Packet::genWriteStartupFilterBiasEstimate( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vec3f gyroBias, + vec3f accelBias, float pressureBias) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,74,%f,%f,%f,%f,%f,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,74,%f,%f,%f,%f,%f,%f,%f" - #endif -, - gyroBias.x, - gyroBias.y, - gyroBias.z, - accelBias.x, - accelBias.y, - accelBias.z, - pressureBias); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,74,%f,%f,%f,%f,%f,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,74,%f,%f,%f,%f,%f,%f,%f" +#endif + , + gyroBias.x, gyroBias.y, gyroBias.z, accelBias.x, accelBias.y, accelBias.z, pressureBias); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadDeltaThetaAndDeltaVelocity(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadDeltaThetaAndDeltaVelocity( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,80"); - #else - size_t length = sprintf(buffer, "$VNRRG,80"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,80"); +#else + size_t length = sprintf(buffer, "$VNRRG,80"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadDeltaThetaAndDeltaVelocityConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadDeltaThetaAndDeltaVelocityConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,82"); - #else - size_t length = sprintf(buffer, "$VNRRG,82"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,82"); +#else + size_t length = sprintf(buffer, "$VNRRG,82"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteDeltaThetaAndDeltaVelocityConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t integrationFrame, uint8_t gyroCompensation, uint8_t accelCompensation) +size_t Packet::genWriteDeltaThetaAndDeltaVelocityConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t integrationFrame, + uint8_t gyroCompensation, uint8_t accelCompensation) { - return genWriteDeltaThetaAndDeltaVelocityConfiguration(errorDetectionMode, buffer, size, integrationFrame, gyroCompensation, accelCompensation, 0); + return genWriteDeltaThetaAndDeltaVelocityConfiguration( + errorDetectionMode, buffer, size, integrationFrame, gyroCompensation, accelCompensation, 0); } -size_t Packet::genWriteDeltaThetaAndDeltaVelocityConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t integrationFrame, uint8_t gyroCompensation, uint8_t accelCompensation, uint8_t earthRateCorrection) +size_t Packet::genWriteDeltaThetaAndDeltaVelocityConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t integrationFrame, + uint8_t gyroCompensation, uint8_t accelCompensation, uint8_t earthRateCorrection) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,82,%u,%u,%u,%u,0" - #else - size_t length = sprintf(buffer, "$VNWRG,82,%u,%u,%u,%u,0" - #endif -, - integrationFrame, - gyroCompensation, - accelCompensation, - earthRateCorrection); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,82,%u,%u,%u,%u,0" +#else + size_t length = sprintf( + buffer, "$VNWRG,82,%u,%u,%u,%u,0" +#endif + , + integrationFrame, gyroCompensation, accelCompensation, earthRateCorrection); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadReferenceVectorConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadReferenceVectorConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,83"); - #else - size_t length = sprintf(buffer, "$VNRRG,83"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,83"); +#else + size_t length = sprintf(buffer, "$VNRRG,83"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteReferenceVectorConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t useMagModel, uint8_t useGravityModel, uint32_t recalcThreshold, float year, vec3d position) +size_t Packet::genWriteReferenceVectorConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint8_t useMagModel, + uint8_t useGravityModel, uint32_t recalcThreshold, float year, vec3d position) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,83,%u,%u,0,0,%u,%f,%f,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,83,%u,%u,0,0,%u,%f,%f,%f,%f" - #endif -, - useMagModel, - useGravityModel, - recalcThreshold, - year, - position.x, - position.y, - position.z); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,83,%u,%u,0,0,%u,%f,%f,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,83,%u,%u,0,0,%u,%f,%f,%f,%f" +#endif + , + useMagModel, useGravityModel, recalcThreshold, year, position.x, position.y, position.z); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadGyroCompensation(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadGyroCompensation( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,84"); - #else - size_t length = sprintf(buffer, "$VNRRG,84"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,84"); +#else + size_t length = sprintf(buffer, "$VNRRG,84"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteGyroCompensation(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, mat3f c, vec3f b) +size_t Packet::genWriteGyroCompensation( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, mat3f c, vec3f b) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,84,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,84,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f" - #endif -, - c.e00, - c.e01, - c.e02, - c.e10, - c.e11, - c.e12, - c.e20, - c.e21, - c.e22, - b.x, - b.y, - b.z); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,84,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,84,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f" +#endif + , + c.e00, c.e01, c.e02, c.e10, c.e11, c.e12, c.e20, c.e21, c.e22, b.x, b.y, b.z); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadImuFilteringConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadImuFilteringConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,85"); - #else - size_t length = sprintf(buffer, "$VNRRG,85"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,85"); +#else + size_t length = sprintf(buffer, "$VNRRG,85"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteImuFilteringConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, 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) +size_t Packet::genWriteImuFilteringConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, 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 VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,85,%u,%u,%u,%u,%u,%u,%u,%u,%u,%u" - #else - size_t length = sprintf(buffer, "$VNWRG,85,%u,%u,%u,%u,%u,%u,%u,%u,%u,%u" - #endif -, - magWindowSize, - accelWindowSize, - gyroWindowSize, - tempWindowSize, - presWindowSize, - magFilterMode, - accelFilterMode, - gyroFilterMode, - tempFilterMode, - presFilterMode); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,85,%u,%u,%u,%u,%u,%u,%u,%u,%u,%u" +#else + size_t length = sprintf( + buffer, "$VNWRG,85,%u,%u,%u,%u,%u,%u,%u,%u,%u,%u" +#endif + , + magWindowSize, accelWindowSize, gyroWindowSize, tempWindowSize, presWindowSize, magFilterMode, + accelFilterMode, gyroFilterMode, tempFilterMode, presFilterMode); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadGpsCompassBaseline(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadGpsCompassBaseline( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,93"); - #else - size_t length = sprintf(buffer, "$VNRRG,93"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,93"); +#else + size_t length = sprintf(buffer, "$VNRRG,93"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteGpsCompassBaseline(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f position, vec3f uncertainty) +size_t Packet::genWriteGpsCompassBaseline( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, vec3f position, + vec3f uncertainty) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,93,%f,%f,%f,%f,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,93,%f,%f,%f,%f,%f,%f" - #endif -, - position.x, - position.y, - position.z, - uncertainty.x, - uncertainty.y, - uncertainty.z); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,93,%f,%f,%f,%f,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,93,%f,%f,%f,%f,%f,%f" +#endif + , + position.x, position.y, position.z, uncertainty.x, uncertainty.y, uncertainty.z); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadGpsCompassEstimatedBaseline(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadGpsCompassEstimatedBaseline( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,97"); - #else - size_t length = sprintf(buffer, "$VNRRG,97"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,97"); +#else + size_t length = sprintf(buffer, "$VNRRG,97"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadImuRateConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadImuRateConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,227"); - #else - size_t length = sprintf(buffer, "$VNRRG,227"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,227"); +#else + size_t length = sprintf(buffer, "$VNRRG,227"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genWriteImuRateConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint16_t imuRate, uint16_t navDivisor, float filterTargetRate, float filterMinRate) +size_t Packet::genWriteImuRateConfiguration( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size, uint16_t imuRate, + uint16_t navDivisor, float filterTargetRate, float filterMinRate) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNWRG,227,%u,%u,%f,%f" - #else - size_t length = sprintf(buffer, "$VNWRG,227,%u,%u,%f,%f" - #endif -, - imuRate, - navDivisor, - filterTargetRate, - filterMinRate); +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s( + buffer, size, "$VNWRG,227,%u,%u,%f,%f" +#else + size_t length = sprintf( + buffer, "$VNWRG,227,%u,%u,%f,%f" +#endif + , + imuRate, navDivisor, filterTargetRate, filterMinRate); - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadYawPitchRollTrueBodyAccelerationAndAngularRates(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadYawPitchRollTrueBodyAccelerationAndAngularRates( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,239"); - #else - size_t length = sprintf(buffer, "$VNRRG,239"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,239"); +#else + size_t length = sprintf(buffer, "$VNRRG,239"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -size_t Packet::genReadYawPitchRollTrueInertialAccelerationAndAngularRates(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +size_t Packet::genReadYawPitchRollTrueInertialAccelerationAndAngularRates( + ErrorDetectionMode errorDetectionMode, char * buffer, size_t size) { - #if VN_HAVE_SECURE_CRT - size_t length = sprintf_s(buffer, size, "$VNRRG,240"); - #else - size_t length = sprintf(buffer, "$VNRRG,240"); - #endif +#if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,240"); +#else + size_t length = sprintf(buffer, "$VNRRG,240"); +#endif - return finalizeCommand(errorDetectionMode, buffer, length); + return finalizeCommand(errorDetectionMode, buffer, length); } -void Packet::parseVNYPR(vec3f* yawPitchRoll) +void Packet::parseVNYPR(vec3f * yawPitchRoll) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - yawPitchRoll->x = ATOFF; NEXT - yawPitchRoll->y = ATOFF; NEXT - yawPitchRoll->z = ATOFF; + yawPitchRoll->x = ATOFF; + NEXT yawPitchRoll->y = ATOFF; + NEXT yawPitchRoll->z = ATOFF; } -void Packet::parseVNQTN(vec4f* quaternion) +void Packet::parseVNQTN(vec4f * quaternion) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - quaternion->x = ATOFF; NEXT - quaternion->y = ATOFF; NEXT - quaternion->z = ATOFF; NEXT - quaternion->w = ATOFF; + quaternion->x = ATOFF; + NEXT quaternion->y = ATOFF; + NEXT quaternion->z = ATOFF; + NEXT quaternion->w = ATOFF; } #ifdef INTERNAL -void Packet::parseVNQTM(vec4f *quaternion, vec3f *magnetic) +void Packet::parseVNQTM(vec4f * quaternion, vec3f * magnetic) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - quaternion->x = ATOFF; NEXT - quaternion->y = ATOFF; NEXT - quaternion->z = ATOFF; NEXT - quaternion->w = ATOFF; NEXT - magnetic->x = ATOFF; NEXT - magnetic->y = ATOFF; NEXT - magnetic->z = ATOFF; + quaternion->x = ATOFF; + NEXT quaternion->y = ATOFF; + NEXT quaternion->z = ATOFF; + NEXT quaternion->w = ATOFF; + NEXT magnetic->x = ATOFF; + NEXT magnetic->y = ATOFF; + NEXT magnetic->z = ATOFF; } -void Packet::parseVNQTA(vec4f* quaternion, vec3f* acceleration) +void Packet::parseVNQTA(vec4f * quaternion, vec3f * acceleration) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - quaternion->x = ATOFF; NEXT - quaternion->y = ATOFF; NEXT - quaternion->z = ATOFF; NEXT - quaternion->w = ATOFF; NEXT - acceleration->x = ATOFF; NEXT - acceleration->y = ATOFF; NEXT - acceleration->z = ATOFF; + quaternion->x = ATOFF; + NEXT quaternion->y = ATOFF; + NEXT quaternion->z = ATOFF; + NEXT quaternion->w = ATOFF; + NEXT acceleration->x = ATOFF; + NEXT acceleration->y = ATOFF; + NEXT acceleration->z = ATOFF; } -void Packet::parseVNQTR(vec4f* quaternion, vec3f* angularRate) +void Packet::parseVNQTR(vec4f * quaternion, vec3f * angularRate) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - quaternion->x = ATOFF; NEXT - quaternion->y = ATOFF; NEXT - quaternion->z = ATOFF; NEXT - quaternion->w = ATOFF; NEXT - angularRate->x = ATOFF; NEXT - angularRate->y = ATOFF; NEXT - angularRate->z = ATOFF; + quaternion->x = ATOFF; + NEXT quaternion->y = ATOFF; + NEXT quaternion->z = ATOFF; + NEXT quaternion->w = ATOFF; + NEXT angularRate->x = ATOFF; + NEXT angularRate->y = ATOFF; + NEXT angularRate->z = ATOFF; } -void Packet::parseVNQMA(vec4f* quaternion, vec3f* magnetic, vec3f* acceleration) +void Packet::parseVNQMA(vec4f * quaternion, vec3f * magnetic, vec3f * acceleration) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - quaternion->x = ATOFF; NEXT - quaternion->y = ATOFF; NEXT - quaternion->z = ATOFF; NEXT - quaternion->w = ATOFF; NEXT - magnetic->x = ATOFF; NEXT - magnetic->y = ATOFF; NEXT - magnetic->z = ATOFF; NEXT - acceleration->x = ATOFF; NEXT - acceleration->y = ATOFF; NEXT - acceleration->z = ATOFF; + quaternion->x = ATOFF; + NEXT quaternion->y = ATOFF; + NEXT quaternion->z = ATOFF; + NEXT quaternion->w = ATOFF; + NEXT magnetic->x = ATOFF; + NEXT magnetic->y = ATOFF; + NEXT magnetic->z = ATOFF; + NEXT acceleration->x = ATOFF; + NEXT acceleration->y = ATOFF; + NEXT acceleration->z = ATOFF; } -void Packet::parseVNQAR(vec4f* quaternion, vec3f* acceleration, vec3f* angularRate) +void Packet::parseVNQAR(vec4f * quaternion, vec3f * acceleration, vec3f * angularRate) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - quaternion->x = ATOFF; NEXT - quaternion->y = ATOFF; NEXT - quaternion->z = ATOFF; NEXT - quaternion->w = ATOFF; NEXT - acceleration->x = ATOFF; NEXT - acceleration->y = ATOFF; NEXT - acceleration->z = ATOFF; NEXT - angularRate->x = ATOFF; NEXT - angularRate->y = ATOFF; NEXT - angularRate->z = ATOFF; + quaternion->x = ATOFF; + NEXT quaternion->y = ATOFF; + NEXT quaternion->z = ATOFF; + NEXT quaternion->w = ATOFF; + NEXT acceleration->x = ATOFF; + NEXT acceleration->y = ATOFF; + NEXT acceleration->z = ATOFF; + NEXT angularRate->x = ATOFF; + NEXT angularRate->y = ATOFF; + NEXT angularRate->z = ATOFF; } #endif -void Packet::parseVNQMR(vec4f* quaternion, vec3f* magnetic, vec3f* acceleration, vec3f* angularRate) +void Packet::parseVNQMR( + vec4f * quaternion, vec3f * magnetic, vec3f * acceleration, vec3f * angularRate) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - quaternion->x = ATOFF; NEXT - quaternion->y = ATOFF; NEXT - quaternion->z = ATOFF; NEXT - quaternion->w = ATOFF; NEXT - magnetic->x = ATOFF; NEXT - magnetic->y = ATOFF; NEXT - magnetic->z = ATOFF; NEXT - acceleration->x = ATOFF; NEXT - acceleration->y = ATOFF; NEXT - acceleration->z = ATOFF; NEXT - angularRate->x = ATOFF; NEXT - angularRate->y = ATOFF; NEXT - angularRate->z = ATOFF; + quaternion->x = ATOFF; + NEXT quaternion->y = ATOFF; + NEXT quaternion->z = ATOFF; + NEXT quaternion->w = ATOFF; + NEXT magnetic->x = ATOFF; + NEXT magnetic->y = ATOFF; + NEXT magnetic->z = ATOFF; + NEXT acceleration->x = ATOFF; + NEXT acceleration->y = ATOFF; + NEXT acceleration->z = ATOFF; + NEXT angularRate->x = ATOFF; + NEXT angularRate->y = ATOFF; + NEXT angularRate->z = ATOFF; } #ifdef INTERNAL -void Packet::parseVNDCM(mat3f* dcm) +void Packet::parseVNDCM(mat3f * dcm) { - // TODO: Implement. - /* + // TODO: Implement. + /* result = strtok(buffer, delims); result = strtok(0, delims); if (result == NULL) @@ -2590,1585 +2502,1695 @@ void Packet::parseVNDCM(mat3f* dcm) #endif -void Packet::parseVNMAG(vec3f* magnetic) +void Packet::parseVNMAG(vec3f * magnetic) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - magnetic->x = ATOFF; NEXT - magnetic->y = ATOFF; NEXT - magnetic->z = ATOFF; + magnetic->x = ATOFF; + NEXT magnetic->y = ATOFF; + NEXT magnetic->z = ATOFF; } -void Packet::parseVNACC(vec3f* acceleration) +void Packet::parseVNACC(vec3f * acceleration) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - acceleration->x = ATOFF; NEXT - acceleration->y = ATOFF; NEXT - acceleration->z = ATOFF; + acceleration->x = ATOFF; + NEXT acceleration->y = ATOFF; + NEXT acceleration->z = ATOFF; } -void Packet::parseVNGYR(vec3f* angularRate) +void Packet::parseVNGYR(vec3f * angularRate) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - angularRate->x = ATOFF; NEXT - angularRate->y = ATOFF; NEXT - angularRate->z = ATOFF; + angularRate->x = ATOFF; + NEXT angularRate->y = ATOFF; + NEXT angularRate->z = ATOFF; } -void Packet::parseVNMAR(vec3f* magnetic, vec3f* acceleration, vec3f* angularRate) +void Packet::parseVNMAR(vec3f * magnetic, vec3f * acceleration, vec3f * angularRate) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - magnetic->x = ATOFF; NEXT - magnetic->y = ATOFF; NEXT - magnetic->z = ATOFF; NEXT - acceleration->x = ATOFF; NEXT - acceleration->y = ATOFF; NEXT - acceleration->z = ATOFF; NEXT - angularRate->x = ATOFF; NEXT - angularRate->y = ATOFF; NEXT - angularRate->z = ATOFF; + magnetic->x = ATOFF; + NEXT magnetic->y = ATOFF; + NEXT magnetic->z = ATOFF; + NEXT acceleration->x = ATOFF; + NEXT acceleration->y = ATOFF; + NEXT acceleration->z = ATOFF; + NEXT angularRate->x = ATOFF; + NEXT angularRate->y = ATOFF; + NEXT angularRate->z = ATOFF; } -void Packet::parseVNYMR(vec3f* yawPitchRoll, vec3f* magnetic, vec3f* acceleration, vec3f* angularRate) +void Packet::parseVNYMR( + vec3f * yawPitchRoll, vec3f * magnetic, vec3f * acceleration, vec3f * angularRate) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - yawPitchRoll->x = ATOFF; NEXT - yawPitchRoll->y = ATOFF; NEXT - yawPitchRoll->z = ATOFF; NEXT - magnetic->x = ATOFF; NEXT - magnetic->y = ATOFF; NEXT - magnetic->z = ATOFF; NEXT - acceleration->x = ATOFF; NEXT - acceleration->y = ATOFF; NEXT - acceleration->z = ATOFF; NEXT - angularRate->x = ATOFF; NEXT - angularRate->y = ATOFF; NEXT - angularRate->z = ATOFF; + yawPitchRoll->x = ATOFF; + NEXT yawPitchRoll->y = ATOFF; + NEXT yawPitchRoll->z = ATOFF; + NEXT magnetic->x = ATOFF; + NEXT magnetic->y = ATOFF; + NEXT magnetic->z = ATOFF; + NEXT acceleration->x = ATOFF; + NEXT acceleration->y = ATOFF; + NEXT acceleration->z = ATOFF; + NEXT angularRate->x = ATOFF; + NEXT angularRate->y = ATOFF; + NEXT angularRate->z = ATOFF; } #ifdef INTERNAL -void Packet::parseVNYCM(vec3f* yawPitchRoll, vec3f* magnetic, vec3f* acceleration, vec3f* angularRate, float* temperature) +void Packet::parseVNYCM( + vec3f * yawPitchRoll, vec3f * magnetic, vec3f * acceleration, vec3f * angularRate, + float * temperature) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - yawPitchRoll->x = ATOFF; NEXT - yawPitchRoll->y = ATOFF; NEXT - yawPitchRoll->z = ATOFF; NEXT - magnetic->x = ATOFF; NEXT - magnetic->y = ATOFF; NEXT - magnetic->z = ATOFF; NEXT - acceleration->x = ATOFF; NEXT - acceleration->y = ATOFF; NEXT - acceleration->z = ATOFF; NEXT - angularRate->x = ATOFF; NEXT - angularRate->y = ATOFF; NEXT - angularRate->z = ATOFF; NEXT - *temperature = ATOFF; + yawPitchRoll->x = ATOFF; + NEXT yawPitchRoll->y = ATOFF; + NEXT yawPitchRoll->z = ATOFF; + NEXT magnetic->x = ATOFF; + NEXT magnetic->y = ATOFF; + NEXT magnetic->z = ATOFF; + NEXT acceleration->x = ATOFF; + NEXT acceleration->y = ATOFF; + NEXT acceleration->z = ATOFF; + NEXT angularRate->x = ATOFF; + NEXT angularRate->y = ATOFF; + NEXT angularRate->z = ATOFF; + NEXT * temperature = ATOFF; } #endif -void Packet::parseVNYBA(vec3f* yawPitchRoll, vec3f* accelerationBody, vec3f* angularRate) +void Packet::parseVNYBA(vec3f * yawPitchRoll, vec3f * accelerationBody, vec3f * angularRate) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - yawPitchRoll->x = ATOFF; NEXT - yawPitchRoll->y = ATOFF; NEXT - yawPitchRoll->z = ATOFF; NEXT - accelerationBody->x = ATOFF; NEXT - accelerationBody->y = ATOFF; NEXT - accelerationBody->z = ATOFF; NEXT - angularRate->x = ATOFF; NEXT - angularRate->y = ATOFF; NEXT - angularRate->z = ATOFF; + yawPitchRoll->x = ATOFF; + NEXT yawPitchRoll->y = ATOFF; + NEXT yawPitchRoll->z = ATOFF; + NEXT accelerationBody->x = ATOFF; + NEXT accelerationBody->y = ATOFF; + NEXT accelerationBody->z = ATOFF; + NEXT angularRate->x = ATOFF; + NEXT angularRate->y = ATOFF; + NEXT angularRate->z = ATOFF; } -void Packet::parseVNYIA(vec3f* yawPitchRoll, vec3f* accelerationInertial, vec3f* angularRate) +void Packet::parseVNYIA(vec3f * yawPitchRoll, vec3f * accelerationInertial, vec3f * angularRate) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - yawPitchRoll->x = ATOFF; NEXT - yawPitchRoll->y = ATOFF; NEXT - yawPitchRoll->z = ATOFF; NEXT - accelerationInertial->x = ATOFF; NEXT - accelerationInertial->y = ATOFF; NEXT - accelerationInertial->z = ATOFF; NEXT - angularRate->x = ATOFF; NEXT - angularRate->y = ATOFF; NEXT - angularRate->z = ATOFF; + yawPitchRoll->x = ATOFF; + NEXT yawPitchRoll->y = ATOFF; + NEXT yawPitchRoll->z = ATOFF; + NEXT accelerationInertial->x = ATOFF; + NEXT accelerationInertial->y = ATOFF; + NEXT accelerationInertial->z = ATOFF; + NEXT angularRate->x = ATOFF; + NEXT angularRate->y = ATOFF; + NEXT angularRate->z = ATOFF; } #ifdef INTERNAL -void Packet::parseVNICM(vec3f* yawPitchRoll, vec3f* magnetic, vec3f* accelerationInertial, vec3f* angularRate) +void Packet::parseVNICM( + vec3f * yawPitchRoll, vec3f * magnetic, vec3f * accelerationInertial, vec3f * angularRate) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - yawPitchRoll->x = ATOFF; NEXT - yawPitchRoll->y = ATOFF; NEXT - yawPitchRoll->z = ATOFF; NEXT - magnetic->x = ATOFF; NEXT - magnetic->y = ATOFF; NEXT - magnetic->z = ATOFF; NEXT - accelerationInertial->x = ATOFF; NEXT - accelerationInertial->y = ATOFF; NEXT - accelerationInertial->z = ATOFF; NEXT - angularRate->x = ATOFF; NEXT - angularRate->y = ATOFF; NEXT - angularRate->z = ATOFF; + yawPitchRoll->x = ATOFF; + NEXT yawPitchRoll->y = ATOFF; + NEXT yawPitchRoll->z = ATOFF; + NEXT magnetic->x = ATOFF; + NEXT magnetic->y = ATOFF; + NEXT magnetic->z = ATOFF; + NEXT accelerationInertial->x = ATOFF; + NEXT accelerationInertial->y = ATOFF; + NEXT accelerationInertial->z = ATOFF; + NEXT angularRate->x = ATOFF; + NEXT angularRate->y = ATOFF; + NEXT angularRate->z = ATOFF; } #endif -void Packet::parseVNIMU(vec3f* magneticUncompensated, vec3f* accelerationUncompensated, vec3f* angularRateUncompensated, float* temperature, float* pressure) -{ - size_t parseIndex; - - char* result = startAsciiPacketParse(_data, parseIndex); - - magneticUncompensated->x = ATOFF; NEXT - magneticUncompensated->y = ATOFF; NEXT - magneticUncompensated->z = ATOFF; NEXT - accelerationUncompensated->x = ATOFF; NEXT - accelerationUncompensated->y = ATOFF; NEXT - accelerationUncompensated->z = ATOFF; NEXT - angularRateUncompensated->x = ATOFF; NEXT - angularRateUncompensated->y = ATOFF; NEXT - angularRateUncompensated->z = ATOFF; NEXT - *temperature = ATOFF; NEXT - *pressure = ATOFF; -} - -void Packet::parseVNGPS(double* time, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, vec3d* lla, vec3f* nedVel, vec3f* nedAcc, float* speedAcc, float* timeAcc) -{ - size_t parseIndex; - - char* result = startAsciiPacketParse(_data, parseIndex); - - *time = ATOFD; NEXT - *week = ATOU16; NEXT - *gpsFix = ATOU8; NEXT - *numSats = ATOU8; NEXT - lla->x = ATOFD; NEXT - lla->y = ATOFD; NEXT - lla->z = ATOFD; NEXT - nedVel->x = ATOFF; NEXT - nedVel->y = ATOFF; NEXT - nedVel->z = ATOFF; NEXT - nedAcc->x = ATOFF; NEXT - nedAcc->y = ATOFF; NEXT - nedAcc->z = ATOFF; NEXT - *speedAcc = ATOFF; NEXT - *timeAcc = ATOFF; -} - -void Packet::parseVNINS(double* time, uint16_t* week, uint16_t* status, vec3f* yawPitchRoll, vec3d* lla, vec3f* nedVel, float* attUncertainty, float* posUncertainty, float* velUncertainty) -{ - size_t parseIndex; - - char* result = startAsciiPacketParse(_data, parseIndex); - - *time = ATOFD; NEXT - *week = ATOU16; NEXT - *status = ATOU16X; NEXT - yawPitchRoll->x = ATOFF; NEXT - yawPitchRoll->y = ATOFF; NEXT - yawPitchRoll->z = ATOFF; NEXT - lla->x = ATOFD; NEXT - lla->y = ATOFD; NEXT - lla->z = ATOFD; NEXT - nedVel->x = ATOFF; NEXT - nedVel->y = ATOFF; NEXT - nedVel->z = ATOFF; NEXT - *attUncertainty = ATOFF; NEXT - *posUncertainty = ATOFF; NEXT - *velUncertainty = ATOFF; -} - -void Packet::parseVNINE(double* time, uint16_t* week, uint16_t* status, vec3f* ypr, vec3d* position, vec3f* velocity, float* attUncertainty, float* posUncertainty, float* velUncertainty) -{ - size_t parseIndex; - - char* result = startAsciiPacketParse(_data, parseIndex); - - *time = ATOFD; NEXT - *week = ATOU16; NEXT - *status = ATOU16X; NEXT - ypr->x = ATOFF; NEXT - ypr->y = ATOFF; NEXT - ypr->z = ATOFF; NEXT - position->x = ATOFD; NEXT - position->y = ATOFD; NEXT - position->z = ATOFD; NEXT - velocity->x = ATOFF; NEXT - velocity->y = ATOFF; NEXT - velocity->z = ATOFF; NEXT - *attUncertainty = ATOFF; NEXT - *posUncertainty = ATOFF; NEXT - *velUncertainty = ATOFF; -} - -void Packet::parseVNISL(vec3f* ypr, vec3d* lla, vec3f* velocity, vec3f* acceleration, vec3f* angularRate) -{ - size_t parseIndex; - - char* result = startAsciiPacketParse(_data, parseIndex); - - ypr->x = ATOFF; NEXT - ypr->y = ATOFF; NEXT - ypr->z = ATOFF; NEXT - lla->x = ATOFD; NEXT - lla->y = ATOFD; NEXT - lla->z = ATOFD; NEXT - velocity->x = ATOFF; NEXT - velocity->y = ATOFF; NEXT - velocity->z = ATOFF; NEXT - acceleration->x = ATOFF; NEXT - acceleration->y = ATOFF; NEXT - acceleration->z = ATOFF; NEXT - angularRate->x = ATOFF; NEXT - angularRate->y = ATOFF; NEXT - angularRate->z = ATOFF; -} - -void Packet::parseVNISE(vec3f* ypr, vec3d* position, vec3f* velocity, vec3f* acceleration, vec3f* angularRate) -{ - size_t parseIndex; - - char* result = startAsciiPacketParse(_data, parseIndex); - - ypr->x = ATOFF; NEXT - ypr->y = ATOFF; NEXT - ypr->z = ATOFF; NEXT - position->x = ATOFD; NEXT - position->y = ATOFD; NEXT - position->z = ATOFD; NEXT - velocity->x = ATOFF; NEXT - velocity->y = ATOFF; NEXT - velocity->z = ATOFF; NEXT - acceleration->x = ATOFF; NEXT - acceleration->y = ATOFF; NEXT - acceleration->z = ATOFF; NEXT - angularRate->x = ATOFF; NEXT - angularRate->y = ATOFF; NEXT - angularRate->z = ATOFF; +void Packet::parseVNIMU( + vec3f * magneticUncompensated, vec3f * accelerationUncompensated, + vec3f * angularRateUncompensated, float * temperature, float * pressure) +{ + size_t parseIndex; + + char * result = startAsciiPacketParse(_data, parseIndex); + + magneticUncompensated->x = ATOFF; + NEXT magneticUncompensated->y = ATOFF; + NEXT magneticUncompensated->z = ATOFF; + NEXT accelerationUncompensated->x = ATOFF; + NEXT accelerationUncompensated->y = ATOFF; + NEXT accelerationUncompensated->z = ATOFF; + NEXT angularRateUncompensated->x = ATOFF; + NEXT angularRateUncompensated->y = ATOFF; + NEXT angularRateUncompensated->z = ATOFF; + NEXT * temperature = ATOFF; + NEXT * pressure = ATOFF; +} + +void Packet::parseVNGPS( + double * time, uint16_t * week, uint8_t * gpsFix, uint8_t * numSats, vec3d * lla, vec3f * nedVel, + vec3f * nedAcc, float * speedAcc, float * timeAcc) +{ + size_t parseIndex; + + char * result = startAsciiPacketParse(_data, parseIndex); + + *time = ATOFD; + NEXT * week = ATOU16; + NEXT * gpsFix = ATOU8; + NEXT * numSats = ATOU8; + NEXT lla->x = ATOFD; + NEXT lla->y = ATOFD; + NEXT lla->z = ATOFD; + NEXT nedVel->x = ATOFF; + NEXT nedVel->y = ATOFF; + NEXT nedVel->z = ATOFF; + NEXT nedAcc->x = ATOFF; + NEXT nedAcc->y = ATOFF; + NEXT nedAcc->z = ATOFF; + NEXT * speedAcc = ATOFF; + NEXT * timeAcc = ATOFF; +} + +void Packet::parseVNINS( + double * time, uint16_t * week, uint16_t * status, vec3f * yawPitchRoll, vec3d * lla, + vec3f * nedVel, float * attUncertainty, float * posUncertainty, float * velUncertainty) +{ + size_t parseIndex; + + char * result = startAsciiPacketParse(_data, parseIndex); + + *time = ATOFD; + NEXT * week = ATOU16; + NEXT * status = ATOU16X; + NEXT yawPitchRoll->x = ATOFF; + NEXT yawPitchRoll->y = ATOFF; + NEXT yawPitchRoll->z = ATOFF; + NEXT lla->x = ATOFD; + NEXT lla->y = ATOFD; + NEXT lla->z = ATOFD; + NEXT nedVel->x = ATOFF; + NEXT nedVel->y = ATOFF; + NEXT nedVel->z = ATOFF; + NEXT * attUncertainty = ATOFF; + NEXT * posUncertainty = ATOFF; + NEXT * velUncertainty = ATOFF; +} + +void Packet::parseVNINE( + double * time, uint16_t * week, uint16_t * status, vec3f * ypr, vec3d * position, + vec3f * velocity, float * attUncertainty, float * posUncertainty, float * velUncertainty) +{ + size_t parseIndex; + + char * result = startAsciiPacketParse(_data, parseIndex); + + *time = ATOFD; + NEXT * week = ATOU16; + NEXT * status = ATOU16X; + NEXT ypr->x = ATOFF; + NEXT ypr->y = ATOFF; + NEXT ypr->z = ATOFF; + NEXT position->x = ATOFD; + NEXT position->y = ATOFD; + NEXT position->z = ATOFD; + NEXT velocity->x = ATOFF; + NEXT velocity->y = ATOFF; + NEXT velocity->z = ATOFF; + NEXT * attUncertainty = ATOFF; + NEXT * posUncertainty = ATOFF; + NEXT * velUncertainty = ATOFF; +} + +void Packet::parseVNISL( + vec3f * ypr, vec3d * lla, vec3f * velocity, vec3f * acceleration, vec3f * angularRate) +{ + size_t parseIndex; + + char * result = startAsciiPacketParse(_data, parseIndex); + + ypr->x = ATOFF; + NEXT ypr->y = ATOFF; + NEXT ypr->z = ATOFF; + NEXT lla->x = ATOFD; + NEXT lla->y = ATOFD; + NEXT lla->z = ATOFD; + NEXT velocity->x = ATOFF; + NEXT velocity->y = ATOFF; + NEXT velocity->z = ATOFF; + NEXT acceleration->x = ATOFF; + NEXT acceleration->y = ATOFF; + NEXT acceleration->z = ATOFF; + NEXT angularRate->x = ATOFF; + NEXT angularRate->y = ATOFF; + NEXT angularRate->z = ATOFF; +} + +void Packet::parseVNISE( + vec3f * ypr, vec3d * position, vec3f * velocity, vec3f * acceleration, vec3f * angularRate) +{ + size_t parseIndex; + + char * result = startAsciiPacketParse(_data, parseIndex); + + ypr->x = ATOFF; + NEXT ypr->y = ATOFF; + NEXT ypr->z = ATOFF; + NEXT position->x = ATOFD; + NEXT position->y = ATOFD; + NEXT position->z = ATOFD; + NEXT velocity->x = ATOFF; + NEXT velocity->y = ATOFF; + NEXT velocity->z = ATOFF; + NEXT acceleration->x = ATOFF; + NEXT acceleration->y = ATOFF; + NEXT acceleration->z = ATOFF; + NEXT angularRate->x = ATOFF; + NEXT angularRate->y = ATOFF; + NEXT angularRate->z = ATOFF; } #ifdef INTERNAL -void Packet::parseVNRAW(vec3f *magneticVoltage, vec3f *accelerationVoltage, vec3f *angularRateVoltage, float* temperatureVoltage) +void Packet::parseVNRAW( + vec3f * magneticVoltage, vec3f * accelerationVoltage, vec3f * angularRateVoltage, + float * temperatureVoltage) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - magneticVoltage->x = ATOFF; NEXT - magneticVoltage->y = ATOFF; NEXT - magneticVoltage->z = ATOFF; NEXT - accelerationVoltage->x = ATOFF; NEXT - accelerationVoltage->y = ATOFF; NEXT - accelerationVoltage->z = ATOFF; NEXT - angularRateVoltage->x = ATOFF; NEXT - angularRateVoltage->y = ATOFF; NEXT - angularRateVoltage->z = ATOFF; NEXT - *temperatureVoltage = ATOFF; + magneticVoltage->x = ATOFF; + NEXT magneticVoltage->y = ATOFF; + NEXT magneticVoltage->z = ATOFF; + NEXT accelerationVoltage->x = ATOFF; + NEXT accelerationVoltage->y = ATOFF; + NEXT accelerationVoltage->z = ATOFF; + NEXT angularRateVoltage->x = ATOFF; + NEXT angularRateVoltage->y = ATOFF; + NEXT angularRateVoltage->z = ATOFF; + NEXT * temperatureVoltage = ATOFF; } -void Packet::parseVNCMV(vec3f* magneticUncompensated, vec3f* accelerationUncompensated, vec3f* angularRateUncompensated, float* temperature) +void Packet::parseVNCMV( + vec3f * magneticUncompensated, vec3f * accelerationUncompensated, + vec3f * angularRateUncompensated, float * temperature) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - magneticUncompensated->x = ATOFF; NEXT - magneticUncompensated->y = ATOFF; NEXT - magneticUncompensated->z = ATOFF; NEXT - accelerationUncompensated->x = ATOFF; NEXT - accelerationUncompensated->y = ATOFF; NEXT - accelerationUncompensated->z = ATOFF; NEXT - angularRateUncompensated->x = ATOFF; NEXT - angularRateUncompensated->y = ATOFF; NEXT - angularRateUncompensated->z = ATOFF; NEXT - *temperature = ATOFF; + magneticUncompensated->x = ATOFF; + NEXT magneticUncompensated->y = ATOFF; + NEXT magneticUncompensated->z = ATOFF; + NEXT accelerationUncompensated->x = ATOFF; + NEXT accelerationUncompensated->y = ATOFF; + NEXT accelerationUncompensated->z = ATOFF; + NEXT angularRateUncompensated->x = ATOFF; + NEXT angularRateUncompensated->y = ATOFF; + NEXT angularRateUncompensated->z = ATOFF; + NEXT * temperature = ATOFF; } -void Packet::parseVNSTV(vec4f* quaternion, vec3f* angularRateBias) +void Packet::parseVNSTV(vec4f * quaternion, vec3f * angularRateBias) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - quaternion->x = ATOFF; NEXT - quaternion->y = ATOFF; NEXT - quaternion->z = ATOFF; NEXT - quaternion->w = ATOFF; NEXT - angularRateBias->x = ATOFF; NEXT - angularRateBias->y = ATOFF; NEXT - angularRateBias->z = ATOFF; + quaternion->x = ATOFF; + NEXT quaternion->y = ATOFF; + NEXT quaternion->z = ATOFF; + NEXT quaternion->w = ATOFF; + NEXT angularRateBias->x = ATOFF; + NEXT angularRateBias->y = ATOFF; + NEXT angularRateBias->z = ATOFF; } -void Packet::parseVNCOV(vec3f* attitudeVariance, vec3f* angularRateBiasVariance) +void Packet::parseVNCOV(vec3f * attitudeVariance, vec3f * angularRateBiasVariance) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - attitudeVariance->x = ATOFF; NEXT - attitudeVariance->y = ATOFF; NEXT - attitudeVariance->z = ATOFF; NEXT - angularRateBiasVariance->x = ATOFF; NEXT - angularRateBiasVariance->y = ATOFF; NEXT - angularRateBiasVariance->z = ATOFF; + attitudeVariance->x = ATOFF; + NEXT attitudeVariance->y = ATOFF; + NEXT attitudeVariance->z = ATOFF; + NEXT angularRateBiasVariance->x = ATOFF; + NEXT angularRateBiasVariance->y = ATOFF; + NEXT angularRateBiasVariance->z = ATOFF; } #endif -void Packet::parseVNGPE(double* tow, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, vec3d* position, vec3f* velocity, vec3f* posAcc, float* speedAcc, float* timeAcc) +void Packet::parseVNGPE( + double * tow, uint16_t * week, uint8_t * gpsFix, uint8_t * numSats, vec3d * position, + vec3f * velocity, vec3f * posAcc, float * speedAcc, float * timeAcc) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - *tow = ATOFD; NEXT - *week = ATOU16; NEXT - *gpsFix = ATOU8; NEXT - *numSats = ATOU8; NEXT - position->x = ATOFD; NEXT - position->y = ATOFD; NEXT - position->z = ATOFD; NEXT - velocity->x = ATOFF; NEXT - velocity->y = ATOFF; NEXT - velocity->z = ATOFF; NEXT - posAcc->x = ATOFF; NEXT - posAcc->y = ATOFF; NEXT - posAcc->z = ATOFF; NEXT - *speedAcc = ATOFF; NEXT - *timeAcc = ATOFF; + *tow = ATOFD; + NEXT * week = ATOU16; + NEXT * gpsFix = ATOU8; + NEXT * numSats = ATOU8; + NEXT position->x = ATOFD; + NEXT position->y = ATOFD; + NEXT position->z = ATOFD; + NEXT velocity->x = ATOFF; + NEXT velocity->y = ATOFF; + NEXT velocity->z = ATOFF; + NEXT posAcc->x = ATOFF; + NEXT posAcc->y = ATOFF; + NEXT posAcc->z = ATOFF; + NEXT * speedAcc = ATOFF; + NEXT * timeAcc = ATOFF; } -void Packet::parseVNDTV(float* deltaTime, vec3f* deltaTheta, vec3f* deltaVelocity) +void Packet::parseVNDTV(float * deltaTime, vec3f * deltaTheta, vec3f * deltaVelocity) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - *deltaTime = ATOFF; NEXT - deltaTheta->x = ATOFF; NEXT - deltaTheta->y = ATOFF; NEXT - deltaTheta->z = ATOFF; NEXT - deltaVelocity->x = ATOFF; NEXT - deltaVelocity->y = ATOFF; NEXT - deltaVelocity->z = ATOFF; + *deltaTime = ATOFF; + NEXT deltaTheta->x = ATOFF; + NEXT deltaTheta->y = ATOFF; + NEXT deltaTheta->z = ATOFF; + NEXT deltaVelocity->x = ATOFF; + NEXT deltaVelocity->y = ATOFF; + NEXT deltaVelocity->z = ATOFF; } -size_t Packet::computeBinaryPacketLength(char const* startOfPossibleBinaryPacket) +size_t Packet::computeBinaryPacketLength(char const * startOfPossibleBinaryPacket) { - char groupsPresent = startOfPossibleBinaryPacket[1]; - size_t runningPayloadLength = 2; // Start of packet character plus groups present field. - const char* pCurrentGroupField = startOfPossibleBinaryPacket + 2; + char groupsPresent = startOfPossibleBinaryPacket[1]; + size_t runningPayloadLength = 2; // Start of packet character plus groups present field. + const char * pCurrentGroupField = startOfPossibleBinaryPacket + 2; - if (groupsPresent & 0x01) - { - runningPayloadLength += 2 + computeNumOfBytesForBinaryGroupPayload(BINARYGROUP_COMMON, stoh(*reinterpret_cast(pCurrentGroupField))); - pCurrentGroupField += 2; - } + if (groupsPresent & 0x01) { + runningPayloadLength += + 2 + computeNumOfBytesForBinaryGroupPayload( + BINARYGROUP_COMMON, stoh(*reinterpret_cast(pCurrentGroupField))); + pCurrentGroupField += 2; + } - if (groupsPresent & 0x02) - { - runningPayloadLength += 2 + computeNumOfBytesForBinaryGroupPayload(BINARYGROUP_TIME, stoh(*reinterpret_cast(pCurrentGroupField))); - pCurrentGroupField += 2; - } + if (groupsPresent & 0x02) { + runningPayloadLength += + 2 + computeNumOfBytesForBinaryGroupPayload( + BINARYGROUP_TIME, stoh(*reinterpret_cast(pCurrentGroupField))); + pCurrentGroupField += 2; + } - if (groupsPresent & 0x04) - { - runningPayloadLength += 2 + computeNumOfBytesForBinaryGroupPayload(BINARYGROUP_IMU, stoh(*reinterpret_cast(pCurrentGroupField))); - pCurrentGroupField += 2; - } + if (groupsPresent & 0x04) { + runningPayloadLength += + 2 + computeNumOfBytesForBinaryGroupPayload( + BINARYGROUP_IMU, stoh(*reinterpret_cast(pCurrentGroupField))); + pCurrentGroupField += 2; + } - if (groupsPresent & 0x08) - { - runningPayloadLength += 2 + computeNumOfBytesForBinaryGroupPayload(BINARYGROUP_GPS, stoh(*reinterpret_cast(pCurrentGroupField))); - pCurrentGroupField += 2; - } + if (groupsPresent & 0x08) { + runningPayloadLength += + 2 + computeNumOfBytesForBinaryGroupPayload( + BINARYGROUP_GPS, stoh(*reinterpret_cast(pCurrentGroupField))); + pCurrentGroupField += 2; + } - if (groupsPresent & 0x10) - { - runningPayloadLength += 2 + computeNumOfBytesForBinaryGroupPayload(BINARYGROUP_ATTITUDE, stoh(*reinterpret_cast(pCurrentGroupField))); - pCurrentGroupField += 2; - } + if (groupsPresent & 0x10) { + runningPayloadLength += + 2 + computeNumOfBytesForBinaryGroupPayload( + BINARYGROUP_ATTITUDE, stoh(*reinterpret_cast(pCurrentGroupField))); + pCurrentGroupField += 2; + } - if (groupsPresent & 0x20) - { - runningPayloadLength += 2 + computeNumOfBytesForBinaryGroupPayload(BINARYGROUP_INS, stoh(*reinterpret_cast(pCurrentGroupField))); - pCurrentGroupField += 2; - } + if (groupsPresent & 0x20) { + runningPayloadLength += + 2 + computeNumOfBytesForBinaryGroupPayload( + BINARYGROUP_INS, stoh(*reinterpret_cast(pCurrentGroupField))); + pCurrentGroupField += 2; + } - if (groupsPresent & BINARYGROUP_GPS2) - { - runningPayloadLength += 2 + computeNumOfBytesForBinaryGroupPayload(BINARYGROUP_GPS2, stoh(*reinterpret_cast(pCurrentGroupField))); - pCurrentGroupField += 2; - } + if (groupsPresent & BINARYGROUP_GPS2) { + runningPayloadLength += + 2 + computeNumOfBytesForBinaryGroupPayload( + BINARYGROUP_GPS2, stoh(*reinterpret_cast(pCurrentGroupField))); + pCurrentGroupField += 2; + } - return runningPayloadLength + 2; // Add 2 bytes for CRC. + return runningPayloadLength + 2; // Add 2 bytes for CRC. } size_t Packet::computeNumOfBytesForBinaryGroupPayload(BinaryGroup group, uint16_t groupField) { - size_t runningLength = 0; + size_t runningLength = 0; - // Determine which group is present. - size_t groupIndex = 0; - for (size_t i = 0; i < 8; i++, groupIndex++) - { - if ((static_cast(group) >> i) & 0x01) - break; - } + // Determine which group is present. + size_t groupIndex = 0; + for (size_t i = 0; i < 8; i++, groupIndex++) { + if ((static_cast(group) >> i) & 0x01) break; + } - for (size_t i = 0; i < sizeof(uint16_t) * 8; i++) - { - if ((groupField >> i) & 1) - { - runningLength += BinaryGroupLengths[groupIndex][i]; - } - } + for (size_t i = 0; i < sizeof(uint16_t) * 8; i++) { + if ((groupField >> i) & 1) { + runningLength += BinaryGroupLengths[groupIndex][i]; + } + } - return runningLength; + return runningLength; } SensorError Packet::parseError() { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - return static_cast(to_uint8_from_hexstr(result)); + return static_cast(to_uint8_from_hexstr(result)); } -uint8_t Packet::groups() -{ - return _data[1]; -} +uint8_t Packet::groups() { return _data[1]; } uint16_t Packet::groupField(size_t index) { - uint16_t gf; - char* groupStart = _data + index * sizeof(uint16_t) + 2; + uint16_t gf; + char * groupStart = _data + index * sizeof(uint16_t) + 2; - std::memcpy(&gf, groupStart, sizeof(uint16_t)); + std::memcpy(&gf, groupStart, sizeof(uint16_t)); - return stoh(gf); + return stoh(gf); } void Packet::parseBinaryOutput( - uint16_t* asyncMode, - uint16_t* rateDivisor, - uint16_t* outputGroup, - uint16_t* commonField, - uint16_t* timeField, - uint16_t* imuField, - uint16_t* gpsField, - uint16_t* attitudeField, - uint16_t* insField, - uint16_t* gps2Field) -{ - size_t parseIndex; - - char *result = startAsciiPacketParse(_data, parseIndex); NEXT - - *commonField = 0; - *timeField = 0; - *imuField = 0; - *gpsField = 0; - *attitudeField = 0; - *insField = 0; - *gps2Field = 0; - - *asyncMode = ATOU16; NEXT - *rateDivisor = ATOU16; NEXT - *outputGroup = ATOU16; - if (*outputGroup & 0x0001) - { - NEXT - *commonField = ATOU16; - } - if (*outputGroup & 0x0002) - { - NEXT - *timeField = ATOU16; - } - if (*outputGroup & 0x0004) - { - NEXT - *imuField = ATOU16; - } - if (*outputGroup & 0x0008) - { - NEXT - *gpsField = ATOU16; - } - if (*outputGroup & 0x0010) - { - NEXT - *attitudeField = ATOU16; - } - if (*outputGroup & 0x0020) - { - NEXT - *insField = ATOU16; - } - if(*outputGroup & 0x0040) { - NEXT - *gps2Field = ATOU16; - } -} - -void Packet::parseUserTag(char* tag) + uint16_t * asyncMode, uint16_t * rateDivisor, uint16_t * outputGroup, uint16_t * commonField, + uint16_t * timeField, uint16_t * imuField, uint16_t * gpsField, uint16_t * attitudeField, + uint16_t * insField, uint16_t * gps2Field) { - size_t parseIndex; - - char* result = startAsciiPacketParse(_data, parseIndex); - - if (*(result + strlen(result) + 1) == '*') - { - tag[0] = '\0'; - return; - } + size_t parseIndex; - NEXT - - #if defined(_MSC_VER) - //Unable to use strcpy_s since we do not have length of the output array. - #pragma warning(push) - #pragma warning(disable:4996) - #endif + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - strcpy(tag, result); + * commonField = 0; + *timeField = 0; + *imuField = 0; + *gpsField = 0; + *attitudeField = 0; + *insField = 0; + *gps2Field = 0; - #if defined(_MSC_VER) - #pragma warning(pop) - #endif + *asyncMode = ATOU16; + NEXT * rateDivisor = ATOU16; + NEXT * outputGroup = ATOU16; + if (*outputGroup & 0x0001) { + NEXT * commonField = ATOU16; + } + if (*outputGroup & 0x0002) { + NEXT * timeField = ATOU16; + } + if (*outputGroup & 0x0004) { + NEXT * imuField = ATOU16; + } + if (*outputGroup & 0x0008) { + NEXT * gpsField = ATOU16; + } + if (*outputGroup & 0x0010) { + NEXT * attitudeField = ATOU16; + } + if (*outputGroup & 0x0020) { + NEXT * insField = ATOU16; + } + if (*outputGroup & 0x0040) { + NEXT * gps2Field = ATOU16; + } } -void Packet::parseModelNumber(char* productName) +void Packet::parseUserTag(char * tag) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - if (*(result + strlen(result) + 1) == '*') - { - productName[0] = '\0'; - return; - } - - NEXT - - #if defined(_MSC_VER) - //Unable to use strcpy_s since we do not have length of the output array. - #pragma warning(push) - #pragma warning(disable:4996) - #endif - - strcpy(productName, result); - - #if defined(_MSC_VER) - #pragma warning(pop) - #endif -} - -void Packet::parseHardwareRevision(uint32_t* revision) -{ - size_t parseIndex; - - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + if (*(result + strlen(result) + 1) == '*') { + tag[0] = '\0'; + return; + } - *revision = ATOU32; -} + NEXT -void Packet::parseSerialNumber(uint32_t* serialNum) -{ - size_t parseIndex; +#if defined(_MSC_VER) +//Unable to use strcpy_s since we do not have length of the output array. +#pragma warning(push) +#pragma warning(disable : 4996) +#endif - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + strcpy(tag, result); - *serialNum = ATOU32; +#if defined(_MSC_VER) +#pragma warning(pop) +#endif } -void Packet::parseFirmwareVersion(char* firmwareVersion) +void Packet::parseModelNumber(char * productName) { - size_t parseIndex; + size_t parseIndex; - char* result = startAsciiPacketParse(_data, parseIndex); + char * result = startAsciiPacketParse(_data, parseIndex); - if (*(result + strlen(result) + 1) == '*') - { - firmwareVersion[0] = '\0'; - return; - } + if (*(result + strlen(result) + 1) == '*') { + productName[0] = '\0'; + return; + } - NEXT + NEXT - #if defined(_MSC_VER) - //Unable to use strcpy_s since we do not have length of the output array. - #pragma warning(push) - #pragma warning(disable:4996) - #endif +#if defined(_MSC_VER) +//Unable to use strcpy_s since we do not have length of the output array. +#pragma warning(push) +#pragma warning(disable : 4996) +#endif - strcpy(firmwareVersion, result); + strcpy(productName, result); - #if defined(_MSC_VER) - #pragma warning(pop) - #endif +#if defined(_MSC_VER) +#pragma warning(pop) +#endif } -void Packet::parseSerialBaudRate(uint32_t* baudrate) +void Packet::parseHardwareRevision(uint32_t * revision) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *baudrate = ATOU32; + * revision = ATOU32; } -void Packet::parseAsyncDataOutputType(uint32_t* ador) +void Packet::parseSerialNumber(uint32_t * serialNum) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *ador = ATOU32; + * serialNum = ATOU32; } -void Packet::parseAsyncDataOutputFrequency(uint32_t* adof) +void Packet::parseFirmwareVersion(char * firmwareVersion) { - size_t parseIndex; - - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + size_t parseIndex; - *adof = ATOU32; -} - -void Packet::parseYawPitchRoll(vec3f* yawPitchRoll) -{ - size_t parseIndex; + char * result = startAsciiPacketParse(_data, parseIndex); - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + if (*(result + strlen(result) + 1) == '*') { + firmwareVersion[0] = '\0'; + return; + } - yawPitchRoll->x = ATOFF; NEXT - yawPitchRoll->y = ATOFF; NEXT - yawPitchRoll->z = ATOFF; -} + NEXT -void Packet::parseAttitudeQuaternion(vec4f* quat) -{ - size_t parseIndex; +#if defined(_MSC_VER) +//Unable to use strcpy_s since we do not have length of the output array. +#pragma warning(push) +#pragma warning(disable : 4996) +#endif - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + strcpy(firmwareVersion, result); - quat->x = ATOFF; NEXT - quat->y = ATOFF; NEXT - quat->z = ATOFF; NEXT - quat->w = ATOFF; +#if defined(_MSC_VER) +#pragma warning(pop) +#endif } -void Packet::parseQuaternionMagneticAccelerationAndAngularRates(vec4f* quat, vec3f* mag, vec3f* accel, vec3f* gyro) +void Packet::parseSerialBaudRate(uint32_t * baudrate) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - quat->x = ATOFF; NEXT - quat->y = ATOFF; NEXT - quat->z = ATOFF; NEXT - quat->w = ATOFF; NEXT - mag->x = ATOFF; NEXT - mag->y = ATOFF; NEXT - mag->z = ATOFF; NEXT - accel->x = ATOFF; NEXT - accel->y = ATOFF; NEXT - accel->z = ATOFF; NEXT - gyro->x = ATOFF; NEXT - gyro->y = ATOFF; NEXT - gyro->z = ATOFF; + * baudrate = ATOU32; } -void Packet::parseMagneticMeasurements(vec3f* mag) +void Packet::parseAsyncDataOutputType(uint32_t * ador) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - mag->x = ATOFF; NEXT - mag->y = ATOFF; NEXT - mag->z = ATOFF; + * ador = ATOU32; } -void Packet::parseAccelerationMeasurements(vec3f* accel) +void Packet::parseAsyncDataOutputFrequency(uint32_t * adof) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - accel->x = ATOFF; NEXT - accel->y = ATOFF; NEXT - accel->z = ATOFF; + * adof = ATOU32; } -void Packet::parseAngularRateMeasurements(vec3f* gyro) +void Packet::parseYawPitchRoll(vec3f * yawPitchRoll) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - gyro->x = ATOFF; NEXT - gyro->y = ATOFF; NEXT - gyro->z = ATOFF; + yawPitchRoll->x = ATOFF; + NEXT yawPitchRoll->y = ATOFF; + NEXT yawPitchRoll->z = ATOFF; } -void Packet::parseMagneticAccelerationAndAngularRates(vec3f* mag, vec3f* accel, vec3f* gyro) +void Packet::parseAttitudeQuaternion(vec4f * quat) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - mag->x = ATOFF; NEXT - mag->y = ATOFF; NEXT - mag->z = ATOFF; NEXT - accel->x = ATOFF; NEXT - accel->y = ATOFF; NEXT - accel->z = ATOFF; NEXT - gyro->x = ATOFF; NEXT - gyro->y = ATOFF; NEXT - gyro->z = ATOFF; + quat->x = ATOFF; + NEXT quat->y = ATOFF; + NEXT quat->z = ATOFF; + NEXT quat->w = ATOFF; } -void Packet::parseMagneticAndGravityReferenceVectors(vec3f* magRef, vec3f* accRef) +void Packet::parseQuaternionMagneticAccelerationAndAngularRates( + vec4f * quat, vec3f * mag, vec3f * accel, vec3f * gyro) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - magRef->x = ATOFF; NEXT - magRef->y = ATOFF; NEXT - magRef->z = ATOFF; NEXT - accRef->x = ATOFF; NEXT - accRef->y = ATOFF; NEXT - accRef->z = ATOFF; + quat->x = ATOFF; + NEXT quat->y = ATOFF; + NEXT quat->z = ATOFF; + NEXT quat->w = ATOFF; + NEXT mag->x = ATOFF; + NEXT mag->y = ATOFF; + NEXT mag->z = ATOFF; + NEXT accel->x = ATOFF; + NEXT accel->y = ATOFF; + NEXT accel->z = ATOFF; + NEXT gyro->x = ATOFF; + NEXT gyro->y = ATOFF; + NEXT gyro->z = ATOFF; } -void Packet::parseFilterMeasurementsVarianceParameters(float* angularWalkVariance, vec3f* angularRateVariance, vec3f* magneticVariance, vec3f* accelerationVariance) +void Packet::parseMagneticMeasurements(vec3f * mag) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *angularWalkVariance = ATOFF; NEXT - angularRateVariance->x = ATOFF; NEXT - angularRateVariance->y = ATOFF; NEXT - angularRateVariance->z = ATOFF; NEXT - magneticVariance->x = ATOFF; NEXT - magneticVariance->y = ATOFF; NEXT - magneticVariance->z = ATOFF; NEXT - accelerationVariance->x = ATOFF; NEXT - accelerationVariance->y = ATOFF; NEXT - accelerationVariance->z = ATOFF; + mag->x = ATOFF; + NEXT mag->y = ATOFF; + NEXT mag->z = ATOFF; } -void Packet::parseMagnetometerCompensation(mat3f* c, vec3f* b) +void Packet::parseAccelerationMeasurements(vec3f * accel) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - c->e00 = ATOFF; NEXT - c->e01 = ATOFF; NEXT - c->e02 = ATOFF; NEXT - c->e10 = ATOFF; NEXT - c->e11 = ATOFF; NEXT - c->e12 = ATOFF; NEXT - c->e20 = ATOFF; NEXT - c->e21 = ATOFF; NEXT - c->e22 = ATOFF; NEXT - b->x = ATOFF; NEXT - b->y = ATOFF; NEXT - b->z = ATOFF; + accel->x = ATOFF; + NEXT accel->y = ATOFF; + NEXT accel->z = ATOFF; } -void Packet::parseFilterActiveTuningParameters(float* magneticDisturbanceGain, float* accelerationDisturbanceGain, float* magneticDisturbanceMemory, float* accelerationDisturbanceMemory) +void Packet::parseAngularRateMeasurements(vec3f * gyro) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *magneticDisturbanceGain = ATOFF; NEXT - *accelerationDisturbanceGain = ATOFF; NEXT - *magneticDisturbanceMemory = ATOFF; NEXT - *accelerationDisturbanceMemory = ATOFF; + gyro->x = ATOFF; + NEXT gyro->y = ATOFF; + NEXT gyro->z = ATOFF; } -void Packet::parseAccelerationCompensation(mat3f* c, vec3f* b) +void Packet::parseMagneticAccelerationAndAngularRates(vec3f * mag, vec3f * accel, vec3f * gyro) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - c->e00 = ATOFF; NEXT - c->e01 = ATOFF; NEXT - c->e02 = ATOFF; NEXT - c->e10 = ATOFF; NEXT - c->e11 = ATOFF; NEXT - c->e12 = ATOFF; NEXT - c->e20 = ATOFF; NEXT - c->e21 = ATOFF; NEXT - c->e22 = ATOFF; NEXT - b->x = ATOFF; NEXT - b->y = ATOFF; NEXT - b->z = ATOFF; + mag->x = ATOFF; + NEXT mag->y = ATOFF; + NEXT mag->z = ATOFF; + NEXT accel->x = ATOFF; + NEXT accel->y = ATOFF; + NEXT accel->z = ATOFF; + NEXT gyro->x = ATOFF; + NEXT gyro->y = ATOFF; + NEXT gyro->z = ATOFF; } -void Packet::parseReferenceFrameRotation(mat3f* c) +void Packet::parseMagneticAndGravityReferenceVectors(vec3f * magRef, vec3f * accRef) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - c->e00 = ATOFF; NEXT - c->e01 = ATOFF; NEXT - c->e02 = ATOFF; NEXT - c->e10 = ATOFF; NEXT - c->e11 = ATOFF; NEXT - c->e12 = ATOFF; NEXT - c->e20 = ATOFF; NEXT - c->e21 = ATOFF; NEXT - c->e22 = ATOFF; + magRef->x = ATOFF; + NEXT magRef->y = ATOFF; + NEXT magRef->z = ATOFF; + NEXT accRef->x = ATOFF; + NEXT accRef->y = ATOFF; + NEXT accRef->z = ATOFF; } -void Packet::parseYawPitchRollMagneticAccelerationAndAngularRates(vec3f* yawPitchRoll, vec3f* mag, vec3f* accel, vec3f* gyro) +void Packet::parseFilterMeasurementsVarianceParameters( + float * angularWalkVariance, vec3f * angularRateVariance, vec3f * magneticVariance, + vec3f * accelerationVariance) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - yawPitchRoll->x = ATOFF; NEXT - yawPitchRoll->y = ATOFF; NEXT - yawPitchRoll->z = ATOFF; NEXT - mag->x = ATOFF; NEXT - mag->y = ATOFF; NEXT - mag->z = ATOFF; NEXT - accel->x = ATOFF; NEXT - accel->y = ATOFF; NEXT - accel->z = ATOFF; NEXT - gyro->x = ATOFF; NEXT - gyro->y = ATOFF; NEXT - gyro->z = ATOFF; + * angularWalkVariance = ATOFF; + NEXT angularRateVariance->x = ATOFF; + NEXT angularRateVariance->y = ATOFF; + NEXT angularRateVariance->z = ATOFF; + NEXT magneticVariance->x = ATOFF; + NEXT magneticVariance->y = ATOFF; + NEXT magneticVariance->z = ATOFF; + NEXT accelerationVariance->x = ATOFF; + NEXT accelerationVariance->y = ATOFF; + NEXT accelerationVariance->z = ATOFF; } -void Packet::parseCommunicationProtocolControl(uint8_t* serialCount, uint8_t* serialStatus, uint8_t* spiCount, uint8_t* spiStatus, uint8_t* serialChecksum, uint8_t* spiChecksum, uint8_t* errorMode) +void Packet::parseMagnetometerCompensation(mat3f * c, vec3f * b) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *serialCount = ATOU8; NEXT - *serialStatus = ATOU8; NEXT - *spiCount = ATOU8; NEXT - *spiStatus = ATOU8; NEXT - *serialChecksum = ATOU8; NEXT - *spiChecksum = ATOU8; NEXT - *errorMode = ATOU8; + c->e00 = ATOFF; + NEXT c->e01 = ATOFF; + NEXT c->e02 = ATOFF; + NEXT c->e10 = ATOFF; + NEXT c->e11 = ATOFF; + NEXT c->e12 = ATOFF; + NEXT c->e20 = ATOFF; + NEXT c->e21 = ATOFF; + NEXT c->e22 = ATOFF; + NEXT b->x = ATOFF; + NEXT b->y = ATOFF; + NEXT b->z = ATOFF; } -void Packet::parseSynchronizationControl(uint8_t* syncInMode, uint8_t* syncInEdge, uint16_t* syncInSkipFactor, uint8_t* syncOutMode, uint8_t* syncOutPolarity, uint16_t* syncOutSkipFactor, uint32_t* syncOutPulseWidth) +void Packet::parseFilterActiveTuningParameters( + float * magneticDisturbanceGain, float * accelerationDisturbanceGain, + float * magneticDisturbanceMemory, float * accelerationDisturbanceMemory) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *syncInMode = ATOU8; NEXT - *syncInEdge = ATOU8; NEXT - *syncInSkipFactor = ATOU16; NEXT - NEXT - *syncOutMode = ATOU8; NEXT - *syncOutPolarity = ATOU8; NEXT - *syncOutSkipFactor = ATOU16; NEXT - *syncOutPulseWidth = ATOU32; NEXT + * magneticDisturbanceGain = ATOFF; + NEXT * accelerationDisturbanceGain = ATOFF; + NEXT * magneticDisturbanceMemory = ATOFF; + NEXT * accelerationDisturbanceMemory = ATOFF; } -void Packet::parseSynchronizationStatus(uint32_t* syncInCount, uint32_t* syncInTime, uint32_t* syncOutCount) +void Packet::parseAccelerationCompensation(mat3f * c, vec3f * b) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *syncInCount = ATOU32; NEXT - *syncInTime = ATOU32; NEXT - *syncOutCount = ATOU32; + c->e00 = ATOFF; + NEXT c->e01 = ATOFF; + NEXT c->e02 = ATOFF; + NEXT c->e10 = ATOFF; + NEXT c->e11 = ATOFF; + NEXT c->e12 = ATOFF; + NEXT c->e20 = ATOFF; + NEXT c->e21 = ATOFF; + NEXT c->e22 = ATOFF; + NEXT b->x = ATOFF; + NEXT b->y = ATOFF; + NEXT b->z = ATOFF; } -void Packet::parseFilterBasicControl(uint8_t* magMode, uint8_t* extMagMode, uint8_t* extAccMode, uint8_t* extGyroMode, vec3f* gyroLimit) +void Packet::parseReferenceFrameRotation(mat3f * c) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *magMode = ATOU8; NEXT - *extMagMode = ATOU8; NEXT - *extAccMode = ATOU8; NEXT - *extGyroMode = ATOU8; NEXT - gyroLimit->x = ATOFF; NEXT - gyroLimit->y = ATOFF; NEXT - gyroLimit->z = ATOFF; + c->e00 = ATOFF; + NEXT c->e01 = ATOFF; + NEXT c->e02 = ATOFF; + NEXT c->e10 = ATOFF; + NEXT c->e11 = ATOFF; + NEXT c->e12 = ATOFF; + NEXT c->e20 = ATOFF; + NEXT c->e21 = ATOFF; + NEXT c->e22 = ATOFF; } -void Packet::parseHeaveConfiguration( - float* initialWavePeriod, - float* initialWaveAmplitude, - float* maxWavePeriod, - float* minWaveAmplitude, - float* delayedHeaveCutoffFreq, - float* heaveCutoffFreq, - float* heaveRateCutoffFreq) +void Packet::parseYawPitchRollMagneticAccelerationAndAngularRates( + vec3f * yawPitchRoll, vec3f * mag, vec3f * accel, vec3f * gyro) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *initialWavePeriod = ATOFF; NEXT - *initialWaveAmplitude = ATOFF; NEXT - *maxWavePeriod = ATOFF; NEXT - *minWaveAmplitude = ATOFF; NEXT - *delayedHeaveCutoffFreq = ATOFF; NEXT - *heaveCutoffFreq = ATOFF; NEXT - *heaveRateCutoffFreq = ATOFF; + yawPitchRoll->x = ATOFF; + NEXT yawPitchRoll->y = ATOFF; + NEXT yawPitchRoll->z = ATOFF; + NEXT mag->x = ATOFF; + NEXT mag->y = ATOFF; + NEXT mag->z = ATOFF; + NEXT accel->x = ATOFF; + NEXT accel->y = ATOFF; + NEXT accel->z = ATOFF; + NEXT gyro->x = ATOFF; + NEXT gyro->y = ATOFF; + NEXT gyro->z = ATOFF; } -void Packet::parseVpeBasicControl(uint8_t* enable, uint8_t* headingMode, uint8_t* filteringMode, uint8_t* tuningMode) +void Packet::parseCommunicationProtocolControl( + uint8_t * serialCount, uint8_t * serialStatus, uint8_t * spiCount, uint8_t * spiStatus, + uint8_t * serialChecksum, uint8_t * spiChecksum, uint8_t * errorMode) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *enable = ATOU8; NEXT - *headingMode = ATOU8; NEXT - *filteringMode = ATOU8; NEXT - *tuningMode = ATOU8; + * serialCount = ATOU8; + NEXT * serialStatus = ATOU8; + NEXT * spiCount = ATOU8; + NEXT * spiStatus = ATOU8; + NEXT * serialChecksum = ATOU8; + NEXT * spiChecksum = ATOU8; + NEXT * errorMode = ATOU8; } -void Packet::parseVpeMagnetometerBasicTuning(vec3f* baseTuning, vec3f* adaptiveTuning, vec3f* adaptiveFiltering) +void Packet::parseSynchronizationControl( + uint8_t * syncInMode, uint8_t * syncInEdge, uint16_t * syncInSkipFactor, uint8_t * syncOutMode, + uint8_t * syncOutPolarity, uint16_t * syncOutSkipFactor, uint32_t * syncOutPulseWidth) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - baseTuning->x = ATOFF; NEXT - baseTuning->y = ATOFF; NEXT - baseTuning->z = ATOFF; NEXT - adaptiveTuning->x = ATOFF; NEXT - adaptiveTuning->y = ATOFF; NEXT - adaptiveTuning->z = ATOFF; NEXT - adaptiveFiltering->x = ATOFF; NEXT - adaptiveFiltering->y = ATOFF; NEXT - adaptiveFiltering->z = ATOFF; + * syncInMode = ATOU8; + NEXT * syncInEdge = ATOU8; + NEXT * syncInSkipFactor = ATOU16; + NEXT NEXT * syncOutMode = ATOU8; + NEXT * syncOutPolarity = ATOU8; + NEXT * syncOutSkipFactor = ATOU16; + NEXT * syncOutPulseWidth = ATOU32; + NEXT } -void Packet::parseVpeMagnetometerAdvancedTuning(vec3f* minFiltering, vec3f* maxFiltering, float* maxAdaptRate, float* disturbanceWindow, float* maxTuning) +void Packet::parseSynchronizationStatus( + uint32_t * syncInCount, uint32_t * syncInTime, uint32_t * syncOutCount) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - minFiltering->x = ATOFF; NEXT - minFiltering->y = ATOFF; NEXT - minFiltering->z = ATOFF; NEXT - maxFiltering->x = ATOFF; NEXT - maxFiltering->y = ATOFF; NEXT - maxFiltering->z = ATOFF; NEXT - *maxAdaptRate = ATOFF; NEXT - *disturbanceWindow = ATOFF; NEXT - *maxTuning = ATOFF; + * syncInCount = ATOU32; + NEXT * syncInTime = ATOU32; + NEXT * syncOutCount = ATOU32; } -void Packet::parseVpeAccelerometerBasicTuning(vec3f* baseTuning, vec3f* adaptiveTuning, vec3f* adaptiveFiltering) +void Packet::parseFilterBasicControl( + uint8_t * magMode, uint8_t * extMagMode, uint8_t * extAccMode, uint8_t * extGyroMode, + vec3f * gyroLimit) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - baseTuning->x = ATOFF; NEXT - baseTuning->y = ATOFF; NEXT - baseTuning->z = ATOFF; NEXT - adaptiveTuning->x = ATOFF; NEXT - adaptiveTuning->y = ATOFF; NEXT - adaptiveTuning->z = ATOFF; NEXT - adaptiveFiltering->x = ATOFF; NEXT - adaptiveFiltering->y = ATOFF; NEXT - adaptiveFiltering->z = ATOFF; + * magMode = ATOU8; + NEXT * extMagMode = ATOU8; + NEXT * extAccMode = ATOU8; + NEXT * extGyroMode = ATOU8; + NEXT gyroLimit->x = ATOFF; + NEXT gyroLimit->y = ATOFF; + NEXT gyroLimit->z = ATOFF; } -void Packet::parseVpeAccelerometerAdvancedTuning(vec3f* minFiltering, vec3f* maxFiltering, float* maxAdaptRate, float* disturbanceWindow, float* maxTuning) +void Packet::parseHeaveConfiguration( + float * initialWavePeriod, float * initialWaveAmplitude, float * maxWavePeriod, + float * minWaveAmplitude, float * delayedHeaveCutoffFreq, float * heaveCutoffFreq, + float * heaveRateCutoffFreq) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - minFiltering->x = ATOFF; NEXT - minFiltering->y = ATOFF; NEXT - minFiltering->z = ATOFF; NEXT - maxFiltering->x = ATOFF; NEXT - maxFiltering->y = ATOFF; NEXT - maxFiltering->z = ATOFF; NEXT - *maxAdaptRate = ATOFF; NEXT - *disturbanceWindow = ATOFF; NEXT - *maxTuning = ATOFF; + * initialWavePeriod = ATOFF; + NEXT * initialWaveAmplitude = ATOFF; + NEXT * maxWavePeriod = ATOFF; + NEXT * minWaveAmplitude = ATOFF; + NEXT * delayedHeaveCutoffFreq = ATOFF; + NEXT * heaveCutoffFreq = ATOFF; + NEXT * heaveRateCutoffFreq = ATOFF; } -void Packet::parseVpeGyroBasicTuning(vec3f* angularWalkVariance, vec3f* baseTuning, vec3f* adaptiveTuning) +void Packet::parseVpeBasicControl( + uint8_t * enable, uint8_t * headingMode, uint8_t * filteringMode, uint8_t * tuningMode) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - angularWalkVariance->x = ATOFF; NEXT - angularWalkVariance->y = ATOFF; NEXT - angularWalkVariance->z = ATOFF; NEXT - baseTuning->x = ATOFF; NEXT - baseTuning->y = ATOFF; NEXT - baseTuning->z = ATOFF; NEXT - adaptiveTuning->x = ATOFF; NEXT - adaptiveTuning->y = ATOFF; NEXT - adaptiveTuning->z = ATOFF; + * enable = ATOU8; + NEXT * headingMode = ATOU8; + NEXT * filteringMode = ATOU8; + NEXT * tuningMode = ATOU8; } -void Packet::parseFilterStartupGyroBias(vec3f* bias) +void Packet::parseVpeMagnetometerBasicTuning( + vec3f * baseTuning, vec3f * adaptiveTuning, vec3f * adaptiveFiltering) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - bias->x = ATOFF; NEXT - bias->y = ATOFF; NEXT - bias->z = ATOFF; + baseTuning->x = ATOFF; + NEXT baseTuning->y = ATOFF; + NEXT baseTuning->z = ATOFF; + NEXT adaptiveTuning->x = ATOFF; + NEXT adaptiveTuning->y = ATOFF; + NEXT adaptiveTuning->z = ATOFF; + NEXT adaptiveFiltering->x = ATOFF; + NEXT adaptiveFiltering->y = ATOFF; + NEXT adaptiveFiltering->z = ATOFF; } -void Packet::parseMagnetometerCalibrationControl(uint8_t* hsiMode, uint8_t* hsiOutput, uint8_t* convergeRate) +void Packet::parseVpeMagnetometerAdvancedTuning( + vec3f * minFiltering, vec3f * maxFiltering, float * maxAdaptRate, float * disturbanceWindow, + float * maxTuning) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *hsiMode = ATOU8; NEXT - *hsiOutput = ATOU8; NEXT - *convergeRate = ATOU8; + minFiltering->x = ATOFF; + NEXT minFiltering->y = ATOFF; + NEXT minFiltering->z = ATOFF; + NEXT maxFiltering->x = ATOFF; + NEXT maxFiltering->y = ATOFF; + NEXT maxFiltering->z = ATOFF; + NEXT * maxAdaptRate = ATOFF; + NEXT * disturbanceWindow = ATOFF; + NEXT * maxTuning = ATOFF; } -void Packet::parseCalculatedMagnetometerCalibration(mat3f* c, vec3f* b) +void Packet::parseVpeAccelerometerBasicTuning( + vec3f * baseTuning, vec3f * adaptiveTuning, vec3f * adaptiveFiltering) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - c->e00 = ATOFF; NEXT - c->e01 = ATOFF; NEXT - c->e02 = ATOFF; NEXT - c->e10 = ATOFF; NEXT - c->e11 = ATOFF; NEXT - c->e12 = ATOFF; NEXT - c->e20 = ATOFF; NEXT - c->e21 = ATOFF; NEXT - c->e22 = ATOFF; NEXT - b->x = ATOFF; NEXT - b->y = ATOFF; NEXT - b->z = ATOFF; + baseTuning->x = ATOFF; + NEXT baseTuning->y = ATOFF; + NEXT baseTuning->z = ATOFF; + NEXT adaptiveTuning->x = ATOFF; + NEXT adaptiveTuning->y = ATOFF; + NEXT adaptiveTuning->z = ATOFF; + NEXT adaptiveFiltering->x = ATOFF; + NEXT adaptiveFiltering->y = ATOFF; + NEXT adaptiveFiltering->z = ATOFF; } -void Packet::parseIndoorHeadingModeControl(float* maxRateError) +void Packet::parseVpeAccelerometerAdvancedTuning( + vec3f * minFiltering, vec3f * maxFiltering, float * maxAdaptRate, float * disturbanceWindow, + float * maxTuning) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *maxRateError = ATOFF; NEXT + minFiltering->x = ATOFF; + NEXT minFiltering->y = ATOFF; + NEXT minFiltering->z = ATOFF; + NEXT maxFiltering->x = ATOFF; + NEXT maxFiltering->y = ATOFF; + NEXT maxFiltering->z = ATOFF; + NEXT * maxAdaptRate = ATOFF; + NEXT * disturbanceWindow = ATOFF; + NEXT * maxTuning = ATOFF; } -void Packet::parseVelocityCompensationMeasurement(vec3f* velocity) +void Packet::parseVpeGyroBasicTuning( + vec3f * angularWalkVariance, vec3f * baseTuning, vec3f * adaptiveTuning) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - velocity->x = ATOFF; NEXT - velocity->y = ATOFF; NEXT - velocity->z = ATOFF; + angularWalkVariance->x = ATOFF; + NEXT angularWalkVariance->y = ATOFF; + NEXT angularWalkVariance->z = ATOFF; + NEXT baseTuning->x = ATOFF; + NEXT baseTuning->y = ATOFF; + NEXT baseTuning->z = ATOFF; + NEXT adaptiveTuning->x = ATOFF; + NEXT adaptiveTuning->y = ATOFF; + NEXT adaptiveTuning->z = ATOFF; } -void Packet::parseVelocityCompensationControl(uint8_t* mode, float* velocityTuning, float* rateTuning) +void Packet::parseFilterStartupGyroBias(vec3f * bias) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *mode = ATOU8; NEXT - *velocityTuning = ATOFF; NEXT - *rateTuning = ATOFF; + bias->x = ATOFF; + NEXT bias->y = ATOFF; + NEXT bias->z = ATOFF; } -void Packet::parseVelocityCompensationStatus(float* x, float* xDot, vec3f* accelOffset, vec3f* omega) +void Packet::parseMagnetometerCalibrationControl( + uint8_t * hsiMode, uint8_t * hsiOutput, uint8_t * convergeRate) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *x = ATOFF; NEXT - *xDot = ATOFF; NEXT - accelOffset->x = ATOFF; NEXT - accelOffset->y = ATOFF; NEXT - accelOffset->z = ATOFF; NEXT - omega->x = ATOFF; NEXT - omega->y = ATOFF; NEXT - omega->z = ATOFF; + * hsiMode = ATOU8; + NEXT * hsiOutput = ATOU8; + NEXT * convergeRate = ATOU8; } -void Packet::parseImuMeasurements(vec3f* mag, vec3f* accel, vec3f* gyro, float* temp, float* pressure) +void Packet::parseCalculatedMagnetometerCalibration(mat3f * c, vec3f * b) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - mag->x = ATOFF; NEXT - mag->y = ATOFF; NEXT - mag->z = ATOFF; NEXT - accel->x = ATOFF; NEXT - accel->y = ATOFF; NEXT - accel->z = ATOFF; NEXT - gyro->x = ATOFF; NEXT - gyro->y = ATOFF; NEXT - gyro->z = ATOFF; NEXT - *temp = ATOFF; NEXT - *pressure = ATOFF; + c->e00 = ATOFF; + NEXT c->e01 = ATOFF; + NEXT c->e02 = ATOFF; + NEXT c->e10 = ATOFF; + NEXT c->e11 = ATOFF; + NEXT c->e12 = ATOFF; + NEXT c->e20 = ATOFF; + NEXT c->e21 = ATOFF; + NEXT c->e22 = ATOFF; + NEXT b->x = ATOFF; + NEXT b->y = ATOFF; + NEXT b->z = ATOFF; } -void Packet::parseGpsConfiguration(uint8_t* mode, uint8_t* ppsSource) +void Packet::parseIndoorHeadingModeControl(float * maxRateError) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *mode = ATOU8; NEXT - *ppsSource = ATOU8; NEXT - NEXT - NEXT + * maxRateError = ATOFF; + NEXT } -void Packet::parseGpsConfiguration(uint8_t* mode, uint8_t* ppsSource, uint8_t* rate, uint8_t* antPow) +void Packet::parseVelocityCompensationMeasurement(vec3f * velocity) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *mode = ATOU8; NEXT - *ppsSource = ATOU8; NEXT - *rate = ATOU8; NEXT - NEXT - *antPow = ATOU8; + velocity->x = ATOFF; + NEXT velocity->y = ATOFF; + NEXT velocity->z = ATOFF; } -void Packet::parseGpsAntennaOffset(vec3f* position) +void Packet::parseVelocityCompensationControl( + uint8_t * mode, float * velocityTuning, float * rateTuning) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - position->x = ATOFF; NEXT - position->y = ATOFF; NEXT - position->z = ATOFF; + * mode = ATOU8; + NEXT * velocityTuning = ATOFF; + NEXT * rateTuning = ATOFF; } -void Packet::parseGpsSolutionLla(double* time, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, vec3d* lla, vec3f* nedVel, vec3f* nedAcc, float* speedAcc, float* timeAcc) +void Packet::parseVelocityCompensationStatus( + float * x, float * xDot, vec3f * accelOffset, vec3f * omega) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *time = ATOFD; NEXT - *week = ATOU16; NEXT - *gpsFix = ATOU8; NEXT - *numSats = ATOU8; NEXT - lla->x = ATOFD; NEXT - lla->y = ATOFD; NEXT - lla->z = ATOFD; NEXT - nedVel->x = ATOFF; NEXT - nedVel->y = ATOFF; NEXT - nedVel->z = ATOFF; NEXT - nedAcc->x = ATOFF; NEXT - nedAcc->y = ATOFF; NEXT - nedAcc->z = ATOFF; NEXT - *speedAcc = ATOFF; NEXT - *timeAcc = ATOFF; + * x = ATOFF; + NEXT * xDot = ATOFF; + NEXT accelOffset->x = ATOFF; + NEXT accelOffset->y = ATOFF; + NEXT accelOffset->z = ATOFF; + NEXT omega->x = ATOFF; + NEXT omega->y = ATOFF; + NEXT omega->z = ATOFF; } -void Packet::parseGpsSolutionEcef(double* tow, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, vec3d* position, vec3f* velocity, vec3f* posAcc, float* speedAcc, float* timeAcc) +void Packet::parseImuMeasurements( + vec3f * mag, vec3f * accel, vec3f * gyro, float * temp, float * pressure) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *tow = ATOFD; NEXT - *week = ATOU16; NEXT - *gpsFix = ATOU8; NEXT - *numSats = ATOU8; NEXT - position->x = ATOFD; NEXT - position->y = ATOFD; NEXT - position->z = ATOFD; NEXT - velocity->x = ATOFF; NEXT - velocity->y = ATOFF; NEXT - velocity->z = ATOFF; NEXT - posAcc->x = ATOFF; NEXT - posAcc->y = ATOFF; NEXT - posAcc->z = ATOFF; NEXT - *speedAcc = ATOFF; NEXT - *timeAcc = ATOFF; + mag->x = ATOFF; + NEXT mag->y = ATOFF; + NEXT mag->z = ATOFF; + NEXT accel->x = ATOFF; + NEXT accel->y = ATOFF; + NEXT accel->z = ATOFF; + NEXT gyro->x = ATOFF; + NEXT gyro->y = ATOFF; + NEXT gyro->z = ATOFF; + NEXT * temp = ATOFF; + NEXT * pressure = ATOFF; } -void Packet::parseInsSolutionLla(double* time, uint16_t* week, uint16_t* status, vec3f* yawPitchRoll, vec3d* position, vec3f* nedVel, float* attUncertainty, float* posUncertainty, float* velUncertainty) +void Packet::parseGpsConfiguration(uint8_t * mode, uint8_t * ppsSource) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *time = ATOFD; NEXT - *week = ATOU16; NEXT - *status = ATOU16X; NEXT - yawPitchRoll->x = ATOFF; NEXT - yawPitchRoll->y = ATOFF; NEXT - yawPitchRoll->z = ATOFF; NEXT - position->x = ATOFD; NEXT - position->y = ATOFD; NEXT - position->z = ATOFD; NEXT - nedVel->x = ATOFF; NEXT - nedVel->y = ATOFF; NEXT - nedVel->z = ATOFF; NEXT - *attUncertainty = ATOFF; NEXT - *posUncertainty = ATOFF; NEXT - *velUncertainty = ATOFF; + * mode = ATOU8; + NEXT * ppsSource = ATOU8; + NEXT NEXT NEXT } -void Packet::parseInsSolutionEcef(double* time, uint16_t* week, uint16_t* status, vec3f* yawPitchRoll, vec3d* position, vec3f* velocity, float* attUncertainty, float* posUncertainty, float* velUncertainty) +void Packet::parseGpsConfiguration( + uint8_t * mode, uint8_t * ppsSource, uint8_t * rate, uint8_t * antPow) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *time = ATOFD; NEXT - *week = ATOU16; NEXT - *status = ATOU16X; NEXT - yawPitchRoll->x = ATOFF; NEXT - yawPitchRoll->y = ATOFF; NEXT - yawPitchRoll->z = ATOFF; NEXT - position->x = ATOFD; NEXT - position->y = ATOFD; NEXT - position->z = ATOFD; NEXT - velocity->x = ATOFF; NEXT - velocity->y = ATOFF; NEXT - velocity->z = ATOFF; NEXT - *attUncertainty = ATOFF; NEXT - *posUncertainty = ATOFF; NEXT - *velUncertainty = ATOFF; + * mode = ATOU8; + NEXT * ppsSource = ATOU8; + NEXT * rate = ATOU8; + NEXT NEXT * antPow = ATOU8; } -void Packet::parseInsBasicConfiguration(uint8_t* scenario, uint8_t* ahrsAiding) +void Packet::parseGpsAntennaOffset(vec3f * position) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *scenario = ATOU8; NEXT - *ahrsAiding = ATOU8; NEXT - NEXT + position->x = ATOFF; + NEXT position->y = ATOFF; + NEXT position->z = ATOFF; } -void Packet::parseInsBasicConfiguration(uint8_t* scenario, uint8_t* ahrsAiding, uint8_t* estBaseline) +void Packet::parseGpsSolutionLla( + double * time, uint16_t * week, uint8_t * gpsFix, uint8_t * numSats, vec3d * lla, vec3f * nedVel, + vec3f * nedAcc, float * speedAcc, float * timeAcc) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *scenario = ATOU8; NEXT - *ahrsAiding = ATOU8; NEXT - *estBaseline = ATOU8; NEXT + * time = ATOFD; + NEXT * week = ATOU16; + NEXT * gpsFix = ATOU8; + NEXT * numSats = ATOU8; + NEXT lla->x = ATOFD; + NEXT lla->y = ATOFD; + NEXT lla->z = ATOFD; + NEXT nedVel->x = ATOFF; + NEXT nedVel->y = ATOFF; + NEXT nedVel->z = ATOFF; + NEXT nedAcc->x = ATOFF; + NEXT nedAcc->y = ATOFF; + NEXT nedAcc->z = ATOFF; + NEXT * speedAcc = ATOFF; + NEXT * timeAcc = ATOFF; } -void Packet::parseInsAdvancedConfiguration(uint8_t* useMag, uint8_t* usePres, uint8_t* posAtt, uint8_t* velAtt, uint8_t* velBias, uint8_t* useFoam, uint8_t* gpsCovType, uint8_t* velCount, float* velInit, float* moveOrigin, float* gpsTimeout, float* deltaLimitPos, float* deltaLimitVel, float* minPosUncertainty, float* minVelUncertainty) +void Packet::parseGpsSolutionEcef( + double * tow, uint16_t * week, uint8_t * gpsFix, uint8_t * numSats, vec3d * position, + vec3f * velocity, vec3f * posAcc, float * speedAcc, float * timeAcc) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *useMag = ATOU8; NEXT - *usePres = ATOU8; NEXT - *posAtt = ATOU8; NEXT - *velAtt = ATOU8; NEXT - *velBias = ATOU8; NEXT - *useFoam = ATOU8; NEXT - *gpsCovType = ATOU8; NEXT - *velCount = ATOU8; NEXT - *velInit = ATOFF; NEXT - *moveOrigin = ATOFF; NEXT - *gpsTimeout = ATOFF; NEXT - *deltaLimitPos = ATOFF; NEXT - *deltaLimitVel = ATOFF; NEXT - *minPosUncertainty = ATOFF; NEXT - *minVelUncertainty = ATOFF; + * tow = ATOFD; + NEXT * week = ATOU16; + NEXT * gpsFix = ATOU8; + NEXT * numSats = ATOU8; + NEXT position->x = ATOFD; + NEXT position->y = ATOFD; + NEXT position->z = ATOFD; + NEXT velocity->x = ATOFF; + NEXT velocity->y = ATOFF; + NEXT velocity->z = ATOFF; + NEXT posAcc->x = ATOFF; + NEXT posAcc->y = ATOFF; + NEXT posAcc->z = ATOFF; + NEXT * speedAcc = ATOFF; + NEXT * timeAcc = ATOFF; } -void Packet::parseInsStateLla(vec3f* yawPitchRoll, vec3d* position, vec3f* velocity, vec3f* accel, vec3f* angularRate) +void Packet::parseInsSolutionLla( + double * time, uint16_t * week, uint16_t * status, vec3f * yawPitchRoll, vec3d * position, + vec3f * nedVel, float * attUncertainty, float * posUncertainty, float * velUncertainty) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - yawPitchRoll->x = ATOFF; NEXT - yawPitchRoll->y = ATOFF; NEXT - yawPitchRoll->z = ATOFF; NEXT - position->x = ATOFD; NEXT - position->y = ATOFD; NEXT - position->z = ATOFD; NEXT - velocity->x = ATOFF; NEXT - velocity->y = ATOFF; NEXT - velocity->z = ATOFF; NEXT - accel->x = ATOFF; NEXT - accel->y = ATOFF; NEXT - accel->z = ATOFF; NEXT - angularRate->x = ATOFF; NEXT - angularRate->y = ATOFF; NEXT - angularRate->z = ATOFF; + * time = ATOFD; + NEXT * week = ATOU16; + NEXT * status = ATOU16X; + NEXT yawPitchRoll->x = ATOFF; + NEXT yawPitchRoll->y = ATOFF; + NEXT yawPitchRoll->z = ATOFF; + NEXT position->x = ATOFD; + NEXT position->y = ATOFD; + NEXT position->z = ATOFD; + NEXT nedVel->x = ATOFF; + NEXT nedVel->y = ATOFF; + NEXT nedVel->z = ATOFF; + NEXT * attUncertainty = ATOFF; + NEXT * posUncertainty = ATOFF; + NEXT * velUncertainty = ATOFF; +} + +void Packet::parseInsSolutionEcef( + double * time, uint16_t * week, uint16_t * status, vec3f * yawPitchRoll, vec3d * position, + vec3f * velocity, float * attUncertainty, float * posUncertainty, float * velUncertainty) +{ + size_t parseIndex; + + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT + + * time = ATOFD; + NEXT * week = ATOU16; + NEXT * status = ATOU16X; + NEXT yawPitchRoll->x = ATOFF; + NEXT yawPitchRoll->y = ATOFF; + NEXT yawPitchRoll->z = ATOFF; + NEXT position->x = ATOFD; + NEXT position->y = ATOFD; + NEXT position->z = ATOFD; + NEXT velocity->x = ATOFF; + NEXT velocity->y = ATOFF; + NEXT velocity->z = ATOFF; + NEXT * attUncertainty = ATOFF; + NEXT * posUncertainty = ATOFF; + NEXT * velUncertainty = ATOFF; +} + +void Packet::parseInsBasicConfiguration(uint8_t * scenario, uint8_t * ahrsAiding) +{ + size_t parseIndex; + + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT + + * scenario = ATOU8; + NEXT * ahrsAiding = ATOU8; + NEXT NEXT +} + +void Packet::parseInsBasicConfiguration( + uint8_t * scenario, uint8_t * ahrsAiding, uint8_t * estBaseline) +{ + size_t parseIndex; + + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT + + * scenario = ATOU8; + NEXT * ahrsAiding = ATOU8; + NEXT * estBaseline = ATOU8; + NEXT +} + +void Packet::parseInsAdvancedConfiguration( + uint8_t * useMag, uint8_t * usePres, uint8_t * posAtt, uint8_t * velAtt, uint8_t * velBias, + uint8_t * useFoam, uint8_t * gpsCovType, uint8_t * velCount, float * velInit, float * moveOrigin, + float * gpsTimeout, float * deltaLimitPos, float * deltaLimitVel, float * minPosUncertainty, + float * minVelUncertainty) +{ + size_t parseIndex; + + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT + + * useMag = ATOU8; + NEXT * usePres = ATOU8; + NEXT * posAtt = ATOU8; + NEXT * velAtt = ATOU8; + NEXT * velBias = ATOU8; + NEXT * useFoam = ATOU8; + NEXT * gpsCovType = ATOU8; + NEXT * velCount = ATOU8; + NEXT * velInit = ATOFF; + NEXT * moveOrigin = ATOFF; + NEXT * gpsTimeout = ATOFF; + NEXT * deltaLimitPos = ATOFF; + NEXT * deltaLimitVel = ATOFF; + NEXT * minPosUncertainty = ATOFF; + NEXT * minVelUncertainty = ATOFF; +} + +void Packet::parseInsStateLla( + vec3f * yawPitchRoll, vec3d * position, vec3f * velocity, vec3f * accel, vec3f * angularRate) +{ + size_t parseIndex; + + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT + + yawPitchRoll->x = ATOFF; + NEXT yawPitchRoll->y = ATOFF; + NEXT yawPitchRoll->z = ATOFF; + NEXT position->x = ATOFD; + NEXT position->y = ATOFD; + NEXT position->z = ATOFD; + NEXT velocity->x = ATOFF; + NEXT velocity->y = ATOFF; + NEXT velocity->z = ATOFF; + NEXT accel->x = ATOFF; + NEXT accel->y = ATOFF; + NEXT accel->z = ATOFF; + NEXT angularRate->x = ATOFF; + NEXT angularRate->y = ATOFF; + NEXT angularRate->z = ATOFF; } -void Packet::parseInsStateEcef(vec3f* yawPitchRoll, vec3d* position, vec3f* velocity, vec3f* accel, vec3f* angularRate) +void Packet::parseInsStateEcef( + vec3f * yawPitchRoll, vec3d * position, vec3f * velocity, vec3f * accel, vec3f * angularRate) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - yawPitchRoll->x = ATOFF; NEXT - yawPitchRoll->y = ATOFF; NEXT - yawPitchRoll->z = ATOFF; NEXT - position->x = ATOFD; NEXT - position->y = ATOFD; NEXT - position->z = ATOFD; NEXT - velocity->x = ATOFF; NEXT - velocity->y = ATOFF; NEXT - velocity->z = ATOFF; NEXT - accel->x = ATOFF; NEXT - accel->y = ATOFF; NEXT - accel->z = ATOFF; NEXT - angularRate->x = ATOFF; NEXT - angularRate->y = ATOFF; NEXT - angularRate->z = ATOFF; + yawPitchRoll->x = ATOFF; + NEXT yawPitchRoll->y = ATOFF; + NEXT yawPitchRoll->z = ATOFF; + NEXT position->x = ATOFD; + NEXT position->y = ATOFD; + NEXT position->z = ATOFD; + NEXT velocity->x = ATOFF; + NEXT velocity->y = ATOFF; + NEXT velocity->z = ATOFF; + NEXT accel->x = ATOFF; + NEXT accel->y = ATOFF; + NEXT accel->z = ATOFF; + NEXT angularRate->x = ATOFF; + NEXT angularRate->y = ATOFF; + NEXT angularRate->z = ATOFF; } -void Packet::parseStartupFilterBiasEstimate(vec3f* gyroBias, vec3f* accelBias, float* pressureBias) +void Packet::parseStartupFilterBiasEstimate( + vec3f * gyroBias, vec3f * accelBias, float * pressureBias) { - size_t parseIndex; - - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + size_t parseIndex; - gyroBias->x = ATOFF; NEXT - gyroBias->y = ATOFF; NEXT - gyroBias->z = ATOFF; NEXT - accelBias->x = ATOFF; NEXT - accelBias->y = ATOFF; NEXT - accelBias->z = ATOFF; NEXT - *pressureBias = ATOFF; -} + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT -void Packet::parseDeltaThetaAndDeltaVelocity(float* deltaTime, vec3f* deltaTheta, vec3f* deltaVelocity) + gyroBias->x = ATOFF; + NEXT gyroBias->y = ATOFF; + NEXT gyroBias->z = ATOFF; + NEXT accelBias->x = ATOFF; + NEXT accelBias->y = ATOFF; + NEXT accelBias->z = ATOFF; + NEXT * pressureBias = ATOFF; +} + +void Packet::parseDeltaThetaAndDeltaVelocity( + float * deltaTime, vec3f * deltaTheta, vec3f * deltaVelocity) { - size_t parseIndex; - - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + size_t parseIndex; - *deltaTime = ATOFF; NEXT - deltaTheta->x = ATOFF; NEXT - deltaTheta->y = ATOFF; NEXT - deltaTheta->z = ATOFF; NEXT - deltaVelocity->x = ATOFF; NEXT - deltaVelocity->y = ATOFF; NEXT - deltaVelocity->z = ATOFF; -} + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT -void Packet::parseDeltaThetaAndDeltaVelocityConfiguration(uint8_t* integrationFrame, uint8_t* gyroCompensation, uint8_t* accelCompensation) + * deltaTime = ATOFF; + NEXT deltaTheta->x = ATOFF; + NEXT deltaTheta->y = ATOFF; + NEXT deltaTheta->z = ATOFF; + NEXT deltaVelocity->x = ATOFF; + NEXT deltaVelocity->y = ATOFF; + NEXT deltaVelocity->z = ATOFF; +} + +void Packet::parseDeltaThetaAndDeltaVelocityConfiguration( + uint8_t * integrationFrame, uint8_t * gyroCompensation, uint8_t * accelCompensation) { - uint8_t earthRateCorrection; - parseDeltaThetaAndDeltaVelocityConfiguration(integrationFrame, gyroCompensation, accelCompensation, &earthRateCorrection); + uint8_t earthRateCorrection; + parseDeltaThetaAndDeltaVelocityConfiguration( + integrationFrame, gyroCompensation, accelCompensation, &earthRateCorrection); } -void Packet::parseDeltaThetaAndDeltaVelocityConfiguration(uint8_t* integrationFrame, uint8_t* gyroCompensation, uint8_t* accelCompensation, uint8_t* earthRateCorrection) +void Packet::parseDeltaThetaAndDeltaVelocityConfiguration( + uint8_t * integrationFrame, uint8_t * gyroCompensation, uint8_t * accelCompensation, + uint8_t * earthRateCorrection) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *integrationFrame = ATOU8; NEXT - *gyroCompensation = ATOU8; NEXT - *accelCompensation = ATOU8; NEXT - *earthRateCorrection = ATOU8; NEXT + * integrationFrame = ATOU8; + NEXT * gyroCompensation = ATOU8; + NEXT * accelCompensation = ATOU8; + NEXT * earthRateCorrection = ATOU8; + NEXT } -void Packet::parseReferenceVectorConfiguration(uint8_t* useMagModel, uint8_t* useGravityModel, uint32_t* recalcThreshold, float* year, vec3d* position) +void Packet::parseReferenceVectorConfiguration( + uint8_t * useMagModel, uint8_t * useGravityModel, uint32_t * recalcThreshold, float * year, + vec3d * position) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *useMagModel = ATOU8; NEXT - *useGravityModel = ATOU8; NEXT - NEXT - NEXT - *recalcThreshold = ATOU32; NEXT - *year = ATOFF; NEXT - position->x = ATOFD; NEXT - position->y = ATOFD; NEXT - position->z = ATOFD; + * useMagModel = ATOU8; + NEXT * useGravityModel = ATOU8; + NEXT NEXT NEXT * recalcThreshold = ATOU32; + NEXT * year = ATOFF; + NEXT position->x = ATOFD; + NEXT position->y = ATOFD; + NEXT position->z = ATOFD; } -void Packet::parseGyroCompensation(mat3f* c, vec3f* b) +void Packet::parseGyroCompensation(mat3f * c, vec3f * b) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - c->e00 = ATOFF; NEXT - c->e01 = ATOFF; NEXT - c->e02 = ATOFF; NEXT - c->e10 = ATOFF; NEXT - c->e11 = ATOFF; NEXT - c->e12 = ATOFF; NEXT - c->e20 = ATOFF; NEXT - c->e21 = ATOFF; NEXT - c->e22 = ATOFF; NEXT - b->x = ATOFF; NEXT - b->y = ATOFF; NEXT - b->z = ATOFF; + c->e00 = ATOFF; + NEXT c->e01 = ATOFF; + NEXT c->e02 = ATOFF; + NEXT c->e10 = ATOFF; + NEXT c->e11 = ATOFF; + NEXT c->e12 = ATOFF; + NEXT c->e20 = ATOFF; + NEXT c->e21 = ATOFF; + NEXT c->e22 = ATOFF; + NEXT b->x = ATOFF; + NEXT b->y = ATOFF; + NEXT b->z = ATOFF; } -void Packet::parseImuFilteringConfiguration(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) +void Packet::parseImuFilteringConfiguration( + 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) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *magWindowSize = ATOU16; NEXT - *accelWindowSize = ATOU16; NEXT - *gyroWindowSize = ATOU16; NEXT - *tempWindowSize = ATOU16; NEXT - *presWindowSize = ATOU16; NEXT - *magFilterMode = ATOU8; NEXT - *accelFilterMode = ATOU8; NEXT - *gyroFilterMode = ATOU8; NEXT - *tempFilterMode = ATOU8; NEXT - *presFilterMode = ATOU8; + * magWindowSize = ATOU16; + NEXT * accelWindowSize = ATOU16; + NEXT * gyroWindowSize = ATOU16; + NEXT * tempWindowSize = ATOU16; + NEXT * presWindowSize = ATOU16; + NEXT * magFilterMode = ATOU8; + NEXT * accelFilterMode = ATOU8; + NEXT * gyroFilterMode = ATOU8; + NEXT * tempFilterMode = ATOU8; + NEXT * presFilterMode = ATOU8; } -void Packet::parseGpsCompassBaseline(vec3f* position, vec3f* uncertainty) +void Packet::parseGpsCompassBaseline(vec3f * position, vec3f * uncertainty) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - position->x = ATOFF; NEXT - position->y = ATOFF; NEXT - position->z = ATOFF; NEXT - uncertainty->x = ATOFF; NEXT - uncertainty->y = ATOFF; NEXT - uncertainty->z = ATOFF; + position->x = ATOFF; + NEXT position->y = ATOFF; + NEXT position->z = ATOFF; + NEXT uncertainty->x = ATOFF; + NEXT uncertainty->y = ATOFF; + NEXT uncertainty->z = ATOFF; } -void Packet::parseGpsCompassEstimatedBaseline(uint8_t* estBaselineUsed, uint16_t* numMeas, vec3f* position, vec3f* uncertainty) +void Packet::parseGpsCompassEstimatedBaseline( + uint8_t * estBaselineUsed, uint16_t * numMeas, vec3f * position, vec3f * uncertainty) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *estBaselineUsed = ATOU8; NEXT - NEXT - *numMeas = ATOU16; NEXT - position->x = ATOFF; NEXT - position->y = ATOFF; NEXT - position->z = ATOFF; NEXT - uncertainty->x = ATOFF; NEXT - uncertainty->y = ATOFF; NEXT - uncertainty->z = ATOFF; + * estBaselineUsed = ATOU8; + NEXT NEXT * numMeas = ATOU16; + NEXT position->x = ATOFF; + NEXT position->y = ATOFF; + NEXT position->z = ATOFF; + NEXT uncertainty->x = ATOFF; + NEXT uncertainty->y = ATOFF; + NEXT uncertainty->z = ATOFF; } -void Packet::parseImuRateConfiguration(uint16_t* imuRate, uint16_t* navDivisor, float* filterTargetRate, float* filterMinRate) +void Packet::parseImuRateConfiguration( + uint16_t * imuRate, uint16_t * navDivisor, float * filterTargetRate, float * filterMinRate) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - *imuRate = ATOU16; NEXT - *navDivisor = ATOU16; NEXT - *filterTargetRate = ATOFF; NEXT - *filterMinRate = ATOFF; + * imuRate = ATOU16; + NEXT * navDivisor = ATOU16; + NEXT * filterTargetRate = ATOFF; + NEXT * filterMinRate = ATOFF; } -void Packet::parseYawPitchRollTrueBodyAccelerationAndAngularRates(vec3f* yawPitchRoll, vec3f* bodyAccel, vec3f* gyro) +void Packet::parseYawPitchRollTrueBodyAccelerationAndAngularRates( + vec3f * yawPitchRoll, vec3f * bodyAccel, vec3f * gyro) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - yawPitchRoll->x = ATOFF; NEXT - yawPitchRoll->y = ATOFF; NEXT - yawPitchRoll->z = ATOFF; NEXT - bodyAccel->x = ATOFF; NEXT - bodyAccel->y = ATOFF; NEXT - bodyAccel->z = ATOFF; NEXT - gyro->x = ATOFF; NEXT - gyro->y = ATOFF; NEXT - gyro->z = ATOFF; + yawPitchRoll->x = ATOFF; + NEXT yawPitchRoll->y = ATOFF; + NEXT yawPitchRoll->z = ATOFF; + NEXT bodyAccel->x = ATOFF; + NEXT bodyAccel->y = ATOFF; + NEXT bodyAccel->z = ATOFF; + NEXT gyro->x = ATOFF; + NEXT gyro->y = ATOFF; + NEXT gyro->z = ATOFF; } -void Packet::parseYawPitchRollTrueInertialAccelerationAndAngularRates(vec3f* yawPitchRoll, vec3f* inertialAccel, vec3f* gyro) +void Packet::parseYawPitchRollTrueInertialAccelerationAndAngularRates( + vec3f * yawPitchRoll, vec3f * inertialAccel, vec3f * gyro) { - size_t parseIndex; + size_t parseIndex; - char *result = startAsciiPacketParse(_data, parseIndex); NEXT + char * result = startAsciiPacketParse(_data, parseIndex); + NEXT - yawPitchRoll->x = ATOFF; NEXT - yawPitchRoll->y = ATOFF; NEXT - yawPitchRoll->z = ATOFF; NEXT - inertialAccel->x = ATOFF; NEXT - inertialAccel->y = ATOFF; NEXT - inertialAccel->z = ATOFF; NEXT - gyro->x = ATOFF; NEXT - gyro->y = ATOFF; NEXT - gyro->z = ATOFF; + yawPitchRoll->x = ATOFF; + NEXT yawPitchRoll->y = ATOFF; + NEXT yawPitchRoll->z = ATOFF; + NEXT inertialAccel->x = ATOFF; + NEXT inertialAccel->y = ATOFF; + NEXT inertialAccel->z = ATOFF; + NEXT gyro->x = ATOFF; + NEXT gyro->y = ATOFF; + NEXT gyro->z = ATOFF; } -} -} -} +} // namespace uart +} // namespace protocol +} // namespace vn diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/packetfinder.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/packetfinder.cpp index 946a8787..6844077c 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/packetfinder.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/packetfinder.cpp @@ -1,561 +1,527 @@ #include "vn/packetfinder.h" -#include "vn/utilities.h" -#include "vn/error_detection.h" -#include -#include #include +#include +#include + +#include "vn/error_detection.h" +#include "vn/utilities.h" #if PYTHON - #include "boostpython.h" - namespace bp = boost::python; +#include "boostpython.h" +namespace bp = boost::python; #endif using namespace std; using namespace vn::xplat; using namespace vn::data::integrity; -namespace vn { -namespace protocol { -namespace uart { +namespace vn +{ +namespace protocol +{ +namespace uart +{ //char* vnstrtok(char* str, size_t& startIndex); struct BinaryTracker { - size_t possibleStartIndex; - bool groupsPresentFound; - uint8_t groupsPresent; - uint8_t numOfBytesRemainingToHaveAllGroupFields; - size_t numOfBytesRemainingForCompletePacket; - bool startFoundInProvidedDataBuffer; - size_t runningDataIndexOfStart; - vn::xplat::TimeStamp timeFound; - explicit BinaryTracker(size_t possibleStartIndex, size_t runningDataIndex, TimeStamp timeFound_) : - possibleStartIndex(possibleStartIndex), - groupsPresentFound(false), - numOfBytesRemainingToHaveAllGroupFields(0), - numOfBytesRemainingForCompletePacket(0), - startFoundInProvidedDataBuffer(true), - runningDataIndexOfStart(runningDataIndex), - timeFound(timeFound_) - { } + size_t possibleStartIndex; + bool groupsPresentFound; + uint8_t groupsPresent; + uint8_t numOfBytesRemainingToHaveAllGroupFields; + size_t numOfBytesRemainingForCompletePacket; + bool startFoundInProvidedDataBuffer; + size_t runningDataIndexOfStart; + vn::xplat::TimeStamp timeFound; + explicit BinaryTracker(size_t possibleStartIndex, size_t runningDataIndex, TimeStamp timeFound_) + : possibleStartIndex(possibleStartIndex), + groupsPresentFound(false), + numOfBytesRemainingToHaveAllGroupFields(0), + numOfBytesRemainingForCompletePacket(0), + startFoundInProvidedDataBuffer(true), + runningDataIndexOfStart(runningDataIndex), + timeFound(timeFound_) + { + } }; -bool operator==(const BinaryTracker& lhs, const BinaryTracker& rhs) +bool operator==(const BinaryTracker & lhs, const BinaryTracker & rhs) { - return - lhs.possibleStartIndex == rhs.possibleStartIndex && - lhs.groupsPresentFound == rhs.groupsPresentFound && - lhs.groupsPresent == rhs.groupsPresent && - lhs.numOfBytesRemainingToHaveAllGroupFields == rhs.numOfBytesRemainingToHaveAllGroupFields && - lhs.numOfBytesRemainingForCompletePacket == rhs.numOfBytesRemainingForCompletePacket; + return lhs.possibleStartIndex == rhs.possibleStartIndex && + lhs.groupsPresentFound == rhs.groupsPresentFound && + lhs.groupsPresent == rhs.groupsPresent && + lhs.numOfBytesRemainingToHaveAllGroupFields == + rhs.numOfBytesRemainingToHaveAllGroupFields && + lhs.numOfBytesRemainingForCompletePacket == rhs.numOfBytesRemainingForCompletePacket; } struct PacketFinder::Impl { - static const size_t DefaultReceiveBufferSize = 1200; - static const uint8_t AsciiStartChar = '$'; - static const uint8_t BootloaderVersionStartChar = 'V'; - static const uint8_t BinaryStartChar = 0xFA; - static const uint8_t AsciiEndChar1 = '\r'; - static const uint8_t AsciiEndChar2 = '\n'; - static const size_t MaximumSizeExpectedForBinaryPacket = 600; - static const size_t MaximumSizeForBinaryStartAndAllGroupData = 18; - static const size_t MaximumSizeForAsciiPacket = 256; - - struct AsciiTracker - { - bool currentlyBuildingAsciiPacket; - size_t possibleStartOfPacketIndex; - bool asciiEndChar1Found; - size_t runningDataIndexOfStart; - TimeStamp timeFound; - - AsciiTracker() : - currentlyBuildingAsciiPacket(false), - possibleStartOfPacketIndex(0), - asciiEndChar1Found(false), - runningDataIndexOfStart(0) - { } - - void reset() - { - currentlyBuildingAsciiPacket = false; - possibleStartOfPacketIndex = 0; - asciiEndChar1Found = false; - runningDataIndexOfStart = 0; - timeFound = TimeStamp(); - } - }; - - PacketFinder* _backReference; - uint8_t* _buffer; - const size_t _bufferSize; - size_t _bufferAppendLocation; - AsciiTracker _asciiOnDeck; - list _binaryOnDeck; // Collection of possible binary packets we are checking. - size_t _runningDataIndex; // Used for correlating raw data with where the packet was found for the end user. - void* _possiblePacketFoundUserData; - ValidPacketFoundHandler _possiblePacketFoundHandler; - #if PYTHON - /*boost::python::object* _pythonPacketFoundHandler;*/ - PyObject* _pythonPacketFoundHandler; - #endif - - explicit Impl(PacketFinder* backReference) : - _backReference(backReference), - _buffer(new uint8_t[DefaultReceiveBufferSize]), - _bufferSize(DefaultReceiveBufferSize), - _bufferAppendLocation(0), - _runningDataIndex(0), - _possiblePacketFoundUserData(NULL), - _possiblePacketFoundHandler(NULL) - #if PYTHON - , - _pythonPacketFoundHandler(NULL) - #endif - { } - - Impl(PacketFinder* backReference, size_t internalReceiveBufferSize) : - _backReference(backReference), - _buffer(new uint8_t[internalReceiveBufferSize]), - _bufferSize(internalReceiveBufferSize), - _bufferAppendLocation(0), - _runningDataIndex(0), - _possiblePacketFoundUserData(NULL), - _possiblePacketFoundHandler(NULL) - { } - - ~Impl() - { - delete [] _buffer; - } - - void resetTracking() - { - _asciiOnDeck.reset(); - _binaryOnDeck.clear(); - _bufferAppendLocation = 0; - } - - void dataReceived(uint8_t data[], size_t length, bool bootloaderFilter, TimeStamp timestamp) - { - bool asciiStartFoundInProvidedBuffer = false; - bool asciiDoReset = false; - - // Assume that since the _runningDataIndex is unsigned, any overflows - // will naturally go to zero, which is the behavior that we want. - for (size_t i = 0; i < length; i++, _runningDataIndex++) - { - if (data[i] == AsciiStartChar || (bootloaderFilter && (!_asciiOnDeck.currentlyBuildingAsciiPacket && data[i] == BootloaderVersionStartChar))) - { - _asciiOnDeck.reset(); - _asciiOnDeck.currentlyBuildingAsciiPacket = true; - _asciiOnDeck.possibleStartOfPacketIndex = i; - _asciiOnDeck.runningDataIndexOfStart = _runningDataIndex; - _asciiOnDeck.timeFound = timestamp; - - asciiStartFoundInProvidedBuffer = true; - } - else if (_asciiOnDeck.currentlyBuildingAsciiPacket && data[i] == AsciiEndChar1) - { - _asciiOnDeck.asciiEndChar1Found = true; - } - else if (((bootloaderFilter && _asciiOnDeck.currentlyBuildingAsciiPacket) || (!bootloaderFilter && _asciiOnDeck.asciiEndChar1Found)) && data[i] == AsciiEndChar2) - { - // We have a possible data packet. - size_t runningIndexOfPacketStart = _asciiOnDeck.runningDataIndexOfStart; - uint8_t* startOfAsciiPacket = NULL; - size_t packetLength = 0; - - if (asciiStartFoundInProvidedBuffer) - { - // All the packet was in this data buffer so we don't - // need to do any copying. - - startOfAsciiPacket = data + _asciiOnDeck.possibleStartOfPacketIndex; - packetLength = i - _asciiOnDeck.possibleStartOfPacketIndex + 1; - } - else - { - // The packet was split between the running data buffer - // the current data buffer. We need to copy the data - // over before further processing. - - if (_bufferAppendLocation + i < _bufferSize) - { - memcpy(_buffer + _bufferAppendLocation, data, i + 1); - - startOfAsciiPacket = _buffer + _asciiOnDeck.possibleStartOfPacketIndex; - packetLength = _bufferAppendLocation + i + 1 - _asciiOnDeck.possibleStartOfPacketIndex; - } - else - { - // We are about to overflow our buffer. Just fall - // through to reset tracking. - } - } - - if (packetLength > MaximumSizeForAsciiPacket) // confirm valid packet length - packetLength = 0; - - Packet p(reinterpret_cast(startOfAsciiPacket), packetLength); - - if (p.isValid()) - dispatchPacket(p, runningIndexOfPacketStart, _asciiOnDeck.timeFound); - - asciiDoReset = true; - } - else if (_asciiOnDeck.asciiEndChar1Found) - { - // Invalid packet - EndChar2 not immediately after EndChar1 - asciiDoReset = true; - } - else if (i + 1 - _asciiOnDeck.possibleStartOfPacketIndex > MaximumSizeForAsciiPacket) - { - // Invalid packet - length exceeds max packet - asciiDoReset = true; - } - - if (asciiDoReset) // Either processed packet or invalid packet - { - if (_binaryOnDeck.empty()) - resetTracking(); - else - _asciiOnDeck.reset(); - asciiStartFoundInProvidedBuffer = false; - asciiDoReset = false; - } - - // Update all of our binary packets on deck. - queue invalidPackets; - for (list::iterator it = _binaryOnDeck.begin(); it != _binaryOnDeck.end(); ++it) - { - BinaryTracker &ez = (*it); - - if (!ez.groupsPresentFound) - { - // This byte must be the groups present. - ez.groupsPresentFound = true; - ez.groupsPresent = data[i]; - ez.numOfBytesRemainingToHaveAllGroupFields = 2 * countSetBits(data[i]); - - continue; - } - - if (ez.numOfBytesRemainingToHaveAllGroupFields != 0) - { - // We found another byte belonging to this possible binary packet. - ez.numOfBytesRemainingToHaveAllGroupFields--; - - if (ez.numOfBytesRemainingToHaveAllGroupFields == 0) - { - // We have all of the group fields now. - size_t remainingBytesForCompletePacket; - if (ez.startFoundInProvidedDataBuffer) - { - size_t headerLength = i - ez.possibleStartIndex + 1; - remainingBytesForCompletePacket = Packet::computeBinaryPacketLength(reinterpret_cast(data) + ez.possibleStartIndex) - headerLength; - } - else - { - // Not all of the packet's group is inside the caller's provided buffer. - - // Temporarily copy the rest of the packet to the receive buffer - // for computing the size of the packet. - - size_t numOfBytesToCopyIntoReceiveBuffer = i + 1; - size_t headerLength = _bufferAppendLocation - ez.possibleStartIndex + numOfBytesToCopyIntoReceiveBuffer; - - if (_bufferAppendLocation + numOfBytesToCopyIntoReceiveBuffer < _bufferSize) - { - std::memcpy(_buffer + _bufferAppendLocation, data, numOfBytesToCopyIntoReceiveBuffer); - - remainingBytesForCompletePacket = Packet::computeBinaryPacketLength(reinterpret_cast(_buffer) + ez.possibleStartIndex) - headerLength; - } - else - { - // About to overrun our receive buffer! - invalidPackets.push(ez); - - // TODO: Should we just go ahead and clear the ASCII tracker - // and buffer append location? - - continue; - } - } - - if (remainingBytesForCompletePacket > MaximumSizeExpectedForBinaryPacket) - { - // Must be a bad possible binary packet. - invalidPackets.push(ez); - } - else - { - ez.numOfBytesRemainingForCompletePacket = remainingBytesForCompletePacket; - } - } - - continue; - } - - // We are currently collecting data for our packet. - - ez.numOfBytesRemainingForCompletePacket--; - - if (ez.numOfBytesRemainingForCompletePacket == 0) - { - // We have a possible binary packet! - - uint8_t* packetStart; - size_t packetLength; - - if (ez.startFoundInProvidedDataBuffer) - { - // The binary packet exists completely in the user's provided buffer. - packetStart = data + ez.possibleStartIndex; - packetLength = i - ez.possibleStartIndex + 1; - } - else - { - // The packet is split between our receive buffer and the user's buffer. - size_t numOfBytesToCopyIntoReceiveBuffer = i + 1; - - if (_bufferAppendLocation + numOfBytesToCopyIntoReceiveBuffer < _bufferSize) - { - std::memcpy(_buffer + _bufferAppendLocation, data, numOfBytesToCopyIntoReceiveBuffer); - - packetStart = _buffer + ez.possibleStartIndex; - packetLength = _bufferAppendLocation - ez.possibleStartIndex + i + 1; - } - else - { - // About to overrun our receive buffer! - invalidPackets.push(ez); - - continue; - } - } - - if (packetLength > MaximumSizeExpectedForBinaryPacket) // confirm valid packet length - packetLength = 0; - - Packet p(reinterpret_cast(packetStart), packetLength); - - if (!p.isValid()) - { - // Invalid packet! - invalidPackets.push(ez); - } - else - { - // We have a valid binary packet!!!. - - // Copy data out of the tracking lists since we will be resetting them. - BinaryTracker bt = ez; - - invalidPackets = queue(); - resetTracking(); - - dispatchPacket(p, bt.runningDataIndexOfStart, bt.timeFound); - - break; - } - } - } - - // Remove any invalid packets. - while (!invalidPackets.empty()) - { - _binaryOnDeck.remove(invalidPackets.front()); - invalidPackets.pop(); - } - - if (_binaryOnDeck.empty() && !_asciiOnDeck.currentlyBuildingAsciiPacket) - { - _bufferAppendLocation = 0; - } - - if (data[i] == BinaryStartChar) - { - // Possible start of a binary packet. - _binaryOnDeck.push_back(BinaryTracker(i, _runningDataIndex, timestamp)); - } - } - - if (_binaryOnDeck.empty() && !_asciiOnDeck.currentlyBuildingAsciiPacket) - { - // No data to copy over. - return; - } - - // Perform any data copying to our receive buffer. - - size_t dataIndexToStartCopyingFrom = 0; - bool binaryDataToCopyOver = false; - size_t binaryDataMoveOverIndexAdjustment = 0; - - if (!_binaryOnDeck.empty()) - { - binaryDataToCopyOver = true; - - if (_binaryOnDeck.front().startFoundInProvidedDataBuffer) - { - dataIndexToStartCopyingFrom = _binaryOnDeck.front().possibleStartIndex; - binaryDataMoveOverIndexAdjustment = dataIndexToStartCopyingFrom; - } - } - - if (_asciiOnDeck.currentlyBuildingAsciiPacket && asciiStartFoundInProvidedBuffer) - { - if (_asciiOnDeck.possibleStartOfPacketIndex < dataIndexToStartCopyingFrom) - { - binaryDataMoveOverIndexAdjustment -= binaryDataMoveOverIndexAdjustment - _asciiOnDeck.possibleStartOfPacketIndex; - dataIndexToStartCopyingFrom = _asciiOnDeck.possibleStartOfPacketIndex; - } - else if (!binaryDataToCopyOver) - { - dataIndexToStartCopyingFrom = _asciiOnDeck.possibleStartOfPacketIndex; - } - - // Adjust our ASCII index to be based on the recieve buffer. - _asciiOnDeck.possibleStartOfPacketIndex = _bufferAppendLocation + _asciiOnDeck.possibleStartOfPacketIndex - dataIndexToStartCopyingFrom; - } - - // Adjust any binary packet indexes we are currently building. - for (list::iterator it = _binaryOnDeck.begin(); it != _binaryOnDeck.end(); ++it) - { - if ((*it).startFoundInProvidedDataBuffer) - { - (*it).startFoundInProvidedDataBuffer = false; - (*it).possibleStartIndex = (*it).possibleStartIndex - binaryDataMoveOverIndexAdjustment + _bufferAppendLocation; - } - } - - if (_bufferAppendLocation + length - dataIndexToStartCopyingFrom < _bufferSize) - { - // Safe to copy over the data. - - size_t numOfBytesToCopyOver = length - dataIndexToStartCopyingFrom; - uint8_t *copyFromStart = data + dataIndexToStartCopyingFrom; - - std::memcpy(_buffer + _bufferAppendLocation, copyFromStart, numOfBytesToCopyOver); - _bufferAppendLocation += numOfBytesToCopyOver; - } - else - { - // We are about to overflow our buffer. - resetTracking(); - } - } - - void dispatchPacket(Packet &packet, size_t runningDataIndexAtPacketStart, TimeStamp timestamp) - { - if (_possiblePacketFoundHandler != NULL) - { - _possiblePacketFoundHandler(_possiblePacketFoundUserData, packet, runningDataIndexAtPacketStart, timestamp); - } - } + static const size_t DefaultReceiveBufferSize = 1200; + static const uint8_t AsciiStartChar = '$'; + static const uint8_t BootloaderVersionStartChar = 'V'; + static const uint8_t BinaryStartChar = 0xFA; + static const uint8_t AsciiEndChar1 = '\r'; + static const uint8_t AsciiEndChar2 = '\n'; + static const size_t MaximumSizeExpectedForBinaryPacket = 600; + static const size_t MaximumSizeForBinaryStartAndAllGroupData = 18; + static const size_t MaximumSizeForAsciiPacket = 256; + + struct AsciiTracker + { + bool currentlyBuildingAsciiPacket; + size_t possibleStartOfPacketIndex; + bool asciiEndChar1Found; + size_t runningDataIndexOfStart; + TimeStamp timeFound; + + AsciiTracker() + : currentlyBuildingAsciiPacket(false), + possibleStartOfPacketIndex(0), + asciiEndChar1Found(false), + runningDataIndexOfStart(0) + { + } + + void reset() + { + currentlyBuildingAsciiPacket = false; + possibleStartOfPacketIndex = 0; + asciiEndChar1Found = false; + runningDataIndexOfStart = 0; + timeFound = TimeStamp(); + } + }; + + PacketFinder * _backReference; + uint8_t * _buffer; + const size_t _bufferSize; + size_t _bufferAppendLocation; + AsciiTracker _asciiOnDeck; + list _binaryOnDeck; // Collection of possible binary packets we are checking. + size_t + _runningDataIndex; // Used for correlating raw data with where the packet was found for the end user. + void * _possiblePacketFoundUserData; + ValidPacketFoundHandler _possiblePacketFoundHandler; +#if PYTHON + /*boost::python::object* _pythonPacketFoundHandler;*/ + PyObject * _pythonPacketFoundHandler; +#endif + + explicit Impl(PacketFinder * backReference) + : _backReference(backReference), + _buffer(new uint8_t[DefaultReceiveBufferSize]), + _bufferSize(DefaultReceiveBufferSize), + _bufferAppendLocation(0), + _runningDataIndex(0), + _possiblePacketFoundUserData(NULL), + _possiblePacketFoundHandler(NULL) +#if PYTHON + , + _pythonPacketFoundHandler(NULL) +#endif + { + } + + Impl(PacketFinder * backReference, size_t internalReceiveBufferSize) + : _backReference(backReference), + _buffer(new uint8_t[internalReceiveBufferSize]), + _bufferSize(internalReceiveBufferSize), + _bufferAppendLocation(0), + _runningDataIndex(0), + _possiblePacketFoundUserData(NULL), + _possiblePacketFoundHandler(NULL) + { + } + + ~Impl() { delete[] _buffer; } + + void resetTracking() + { + _asciiOnDeck.reset(); + _binaryOnDeck.clear(); + _bufferAppendLocation = 0; + } + + void dataReceived(uint8_t data[], size_t length, bool bootloaderFilter, TimeStamp timestamp) + { + bool asciiStartFoundInProvidedBuffer = false; + bool asciiDoReset = false; + + // Assume that since the _runningDataIndex is unsigned, any overflows + // will naturally go to zero, which is the behavior that we want. + for (size_t i = 0; i < length; i++, _runningDataIndex++) { + if ( + data[i] == AsciiStartChar || + (bootloaderFilter && + (!_asciiOnDeck.currentlyBuildingAsciiPacket && data[i] == BootloaderVersionStartChar))) { + _asciiOnDeck.reset(); + _asciiOnDeck.currentlyBuildingAsciiPacket = true; + _asciiOnDeck.possibleStartOfPacketIndex = i; + _asciiOnDeck.runningDataIndexOfStart = _runningDataIndex; + _asciiOnDeck.timeFound = timestamp; + + asciiStartFoundInProvidedBuffer = true; + } else if (_asciiOnDeck.currentlyBuildingAsciiPacket && data[i] == AsciiEndChar1) { + _asciiOnDeck.asciiEndChar1Found = true; + } else if ( + ((bootloaderFilter && _asciiOnDeck.currentlyBuildingAsciiPacket) || + (!bootloaderFilter && _asciiOnDeck.asciiEndChar1Found)) && + data[i] == AsciiEndChar2) { + // We have a possible data packet. + size_t runningIndexOfPacketStart = _asciiOnDeck.runningDataIndexOfStart; + uint8_t * startOfAsciiPacket = NULL; + size_t packetLength = 0; + + if (asciiStartFoundInProvidedBuffer) { + // All the packet was in this data buffer so we don't + // need to do any copying. + + startOfAsciiPacket = data + _asciiOnDeck.possibleStartOfPacketIndex; + packetLength = i - _asciiOnDeck.possibleStartOfPacketIndex + 1; + } else { + // The packet was split between the running data buffer + // the current data buffer. We need to copy the data + // over before further processing. + + if (_bufferAppendLocation + i < _bufferSize) { + memcpy(_buffer + _bufferAppendLocation, data, i + 1); + + startOfAsciiPacket = _buffer + _asciiOnDeck.possibleStartOfPacketIndex; + packetLength = _bufferAppendLocation + i + 1 - _asciiOnDeck.possibleStartOfPacketIndex; + } else { + // We are about to overflow our buffer. Just fall + // through to reset tracking. + } + } + + if (packetLength > MaximumSizeForAsciiPacket) // confirm valid packet length + packetLength = 0; + + Packet p(reinterpret_cast(startOfAsciiPacket), packetLength); + + if (p.isValid()) dispatchPacket(p, runningIndexOfPacketStart, _asciiOnDeck.timeFound); + + asciiDoReset = true; + } else if (_asciiOnDeck.asciiEndChar1Found) { + // Invalid packet - EndChar2 not immediately after EndChar1 + asciiDoReset = true; + } else if (i + 1 - _asciiOnDeck.possibleStartOfPacketIndex > MaximumSizeForAsciiPacket) { + // Invalid packet - length exceeds max packet + asciiDoReset = true; + } + + if (asciiDoReset) // Either processed packet or invalid packet + { + if (_binaryOnDeck.empty()) + resetTracking(); + else + _asciiOnDeck.reset(); + asciiStartFoundInProvidedBuffer = false; + asciiDoReset = false; + } + + // Update all of our binary packets on deck. + queue invalidPackets; + for (list::iterator it = _binaryOnDeck.begin(); it != _binaryOnDeck.end(); + ++it) { + BinaryTracker & ez = (*it); + + if (!ez.groupsPresentFound) { + // This byte must be the groups present. + ez.groupsPresentFound = true; + ez.groupsPresent = data[i]; + ez.numOfBytesRemainingToHaveAllGroupFields = 2 * countSetBits(data[i]); + + continue; + } + + if (ez.numOfBytesRemainingToHaveAllGroupFields != 0) { + // We found another byte belonging to this possible binary packet. + ez.numOfBytesRemainingToHaveAllGroupFields--; + + if (ez.numOfBytesRemainingToHaveAllGroupFields == 0) { + // We have all of the group fields now. + size_t remainingBytesForCompletePacket; + if (ez.startFoundInProvidedDataBuffer) { + size_t headerLength = i - ez.possibleStartIndex + 1; + remainingBytesForCompletePacket = + Packet::computeBinaryPacketLength( + reinterpret_cast(data) + ez.possibleStartIndex) - + headerLength; + } else { + // Not all of the packet's group is inside the caller's provided buffer. + + // Temporarily copy the rest of the packet to the receive buffer + // for computing the size of the packet. + + size_t numOfBytesToCopyIntoReceiveBuffer = i + 1; + size_t headerLength = + _bufferAppendLocation - ez.possibleStartIndex + numOfBytesToCopyIntoReceiveBuffer; + + if (_bufferAppendLocation + numOfBytesToCopyIntoReceiveBuffer < _bufferSize) { + std::memcpy( + _buffer + _bufferAppendLocation, data, numOfBytesToCopyIntoReceiveBuffer); + + remainingBytesForCompletePacket = + Packet::computeBinaryPacketLength( + reinterpret_cast(_buffer) + ez.possibleStartIndex) - + headerLength; + } else { + // About to overrun our receive buffer! + invalidPackets.push(ez); + + // TODO: Should we just go ahead and clear the ASCII tracker + // and buffer append location? + + continue; + } + } + + if (remainingBytesForCompletePacket > MaximumSizeExpectedForBinaryPacket) { + // Must be a bad possible binary packet. + invalidPackets.push(ez); + } else { + ez.numOfBytesRemainingForCompletePacket = remainingBytesForCompletePacket; + } + } + + continue; + } + + // We are currently collecting data for our packet. + + ez.numOfBytesRemainingForCompletePacket--; + + if (ez.numOfBytesRemainingForCompletePacket == 0) { + // We have a possible binary packet! + + uint8_t * packetStart; + size_t packetLength; + + if (ez.startFoundInProvidedDataBuffer) { + // The binary packet exists completely in the user's provided buffer. + packetStart = data + ez.possibleStartIndex; + packetLength = i - ez.possibleStartIndex + 1; + } else { + // The packet is split between our receive buffer and the user's buffer. + size_t numOfBytesToCopyIntoReceiveBuffer = i + 1; + + if (_bufferAppendLocation + numOfBytesToCopyIntoReceiveBuffer < _bufferSize) { + std::memcpy(_buffer + _bufferAppendLocation, data, numOfBytesToCopyIntoReceiveBuffer); + + packetStart = _buffer + ez.possibleStartIndex; + packetLength = _bufferAppendLocation - ez.possibleStartIndex + i + 1; + } else { + // About to overrun our receive buffer! + invalidPackets.push(ez); + + continue; + } + } + + if (packetLength > MaximumSizeExpectedForBinaryPacket) // confirm valid packet length + packetLength = 0; + + Packet p(reinterpret_cast(packetStart), packetLength); + + if (!p.isValid()) { + // Invalid packet! + invalidPackets.push(ez); + } else { + // We have a valid binary packet!!!. + + // Copy data out of the tracking lists since we will be resetting them. + BinaryTracker bt = ez; + + invalidPackets = queue(); + resetTracking(); + + dispatchPacket(p, bt.runningDataIndexOfStart, bt.timeFound); + + break; + } + } + } + + // Remove any invalid packets. + while (!invalidPackets.empty()) { + _binaryOnDeck.remove(invalidPackets.front()); + invalidPackets.pop(); + } + + if (_binaryOnDeck.empty() && !_asciiOnDeck.currentlyBuildingAsciiPacket) { + _bufferAppendLocation = 0; + } + + if (data[i] == BinaryStartChar) { + // Possible start of a binary packet. + _binaryOnDeck.push_back(BinaryTracker(i, _runningDataIndex, timestamp)); + } + } + + if (_binaryOnDeck.empty() && !_asciiOnDeck.currentlyBuildingAsciiPacket) { + // No data to copy over. + return; + } + + // Perform any data copying to our receive buffer. + + size_t dataIndexToStartCopyingFrom = 0; + bool binaryDataToCopyOver = false; + size_t binaryDataMoveOverIndexAdjustment = 0; + + if (!_binaryOnDeck.empty()) { + binaryDataToCopyOver = true; + + if (_binaryOnDeck.front().startFoundInProvidedDataBuffer) { + dataIndexToStartCopyingFrom = _binaryOnDeck.front().possibleStartIndex; + binaryDataMoveOverIndexAdjustment = dataIndexToStartCopyingFrom; + } + } + + if (_asciiOnDeck.currentlyBuildingAsciiPacket && asciiStartFoundInProvidedBuffer) { + if (_asciiOnDeck.possibleStartOfPacketIndex < dataIndexToStartCopyingFrom) { + binaryDataMoveOverIndexAdjustment -= + binaryDataMoveOverIndexAdjustment - _asciiOnDeck.possibleStartOfPacketIndex; + dataIndexToStartCopyingFrom = _asciiOnDeck.possibleStartOfPacketIndex; + } else if (!binaryDataToCopyOver) { + dataIndexToStartCopyingFrom = _asciiOnDeck.possibleStartOfPacketIndex; + } + + // Adjust our ASCII index to be based on the recieve buffer. + _asciiOnDeck.possibleStartOfPacketIndex = _bufferAppendLocation + + _asciiOnDeck.possibleStartOfPacketIndex - + dataIndexToStartCopyingFrom; + } + + // Adjust any binary packet indexes we are currently building. + for (list::iterator it = _binaryOnDeck.begin(); it != _binaryOnDeck.end(); + ++it) { + if ((*it).startFoundInProvidedDataBuffer) { + (*it).startFoundInProvidedDataBuffer = false; + (*it).possibleStartIndex = + (*it).possibleStartIndex - binaryDataMoveOverIndexAdjustment + _bufferAppendLocation; + } + } + + if (_bufferAppendLocation + length - dataIndexToStartCopyingFrom < _bufferSize) { + // Safe to copy over the data. + + size_t numOfBytesToCopyOver = length - dataIndexToStartCopyingFrom; + uint8_t * copyFromStart = data + dataIndexToStartCopyingFrom; + + std::memcpy(_buffer + _bufferAppendLocation, copyFromStart, numOfBytesToCopyOver); + _bufferAppendLocation += numOfBytesToCopyOver; + } else { + // We are about to overflow our buffer. + resetTracking(); + } + } + + void dispatchPacket(Packet & packet, size_t runningDataIndexAtPacketStart, TimeStamp timestamp) + { + if (_possiblePacketFoundHandler != NULL) { + _possiblePacketFoundHandler( + _possiblePacketFoundUserData, packet, runningDataIndexAtPacketStart, timestamp); + } + } }; #if defined(_MSC_VER) && _MSC_VER <= 1600 - #pragma warning(push) - // Disable VS2010 warning for 'this' used in base member initializer list. - #pragma warning(disable:4355) +#pragma warning(push) +// Disable VS2010 warning for 'this' used in base member initializer list. +#pragma warning(disable : 4355) #endif -PacketFinder::PacketFinder() : - _pi(new Impl(this)) -{ -} +PacketFinder::PacketFinder() : _pi(new Impl(this)) {} -PacketFinder::PacketFinder(size_t internalReceiveBufferSize) : - _pi(new Impl(this, internalReceiveBufferSize)) +PacketFinder::PacketFinder(size_t internalReceiveBufferSize) +: _pi(new Impl(this, internalReceiveBufferSize)) { } #if defined(_MSC_VER) && _MSC_VER <= 1600 - #pragma warning(pop) +#pragma warning(pop) #endif -PacketFinder::~PacketFinder() -{ - delete _pi; -} +PacketFinder::~PacketFinder() { delete _pi; } void PacketFinder::processReceivedData(char data[], size_t length) { - TimeStamp placeholder; + TimeStamp placeholder; - processReceivedData(data, length, false, placeholder); + processReceivedData(data, length, false, placeholder); } void PacketFinder::processReceivedData(char data[], size_t length, bool bootloaderFilter) { - TimeStamp placeholder; + TimeStamp placeholder; - processReceivedData(data, length, bootloaderFilter, placeholder); + processReceivedData(data, length, bootloaderFilter, placeholder); } -void PacketFinder::processReceivedData(char data[], size_t length, bool bootloaderFilter, TimeStamp timestamp) +void PacketFinder::processReceivedData( + char data[], size_t length, bool bootloaderFilter, TimeStamp timestamp) { - _pi->dataReceived(reinterpret_cast(data), length, bootloaderFilter, timestamp); + _pi->dataReceived(reinterpret_cast(data), length, bootloaderFilter, timestamp); } #if PYTHON void PacketFinder::processReceivedData(boost::python::list data) { - //size_t len = boost::python::len(data); - //unique cppData = make_unique(len); + //size_t len = boost::python::len(data); + //unique cppData = make_unique(len); - //for (auto i = 0; i < len; i++) - // cppData[i] = boost::python::extract(data[i]); + //for (auto i = 0; i < len; i++) + // cppData[i] = boost::python::extract(data[i]); - //processReceivedData(cppData.get(), len); + //processReceivedData(cppData.get(), len); } #endif -void PacketFinder::registerPossiblePacketFoundHandler(void* userData, ValidPacketFoundHandler handler) +void PacketFinder::registerPossiblePacketFoundHandler( + void * userData, ValidPacketFoundHandler handler) { - if (_pi->_possiblePacketFoundHandler != NULL) - throw invalid_operation(); + if (_pi->_possiblePacketFoundHandler != NULL) throw invalid_operation(); - _pi->_possiblePacketFoundHandler = handler; - _pi->_possiblePacketFoundUserData = userData; + _pi->_possiblePacketFoundHandler = handler; + _pi->_possiblePacketFoundUserData = userData; } void PacketFinder::unregisterPossiblePacketFoundHandler() { - if (_pi->_possiblePacketFoundHandler == NULL) - throw invalid_operation(); + if (_pi->_possiblePacketFoundHandler == NULL) throw invalid_operation(); - _pi->_possiblePacketFoundHandler = NULL; - _pi->_possiblePacketFoundUserData = NULL; + _pi->_possiblePacketFoundHandler = NULL; + _pi->_possiblePacketFoundUserData = NULL; } #if PYTHON //void PacketFinder::register_packet_found_handler(boost::python::object* callable) -boost::python::object* PacketFinder::register_packet_found_handler(PyObject* callable/*boost::python::object* callable*/) +boost::python::object * PacketFinder::register_packet_found_handler( + PyObject * callable /*boost::python::object* callable*/) { - _pi->_pythonPacketFoundHandler = callable; - callable->ob_refcnt++; - //_pi->_pythonPacketFoundHandler = callable; - //callable->ptr()->ob_refcnt++; - //callable->ptr(); - //_pi->_rawDataReceivedHandlerPython = callable; - //callable->ob_refcnt++; + _pi->_pythonPacketFoundHandler = callable; + callable->ob_refcnt++; + //_pi->_pythonPacketFoundHandler = callable; + //callable->ptr()->ob_refcnt++; + //callable->ptr(); + //_pi->_rawDataReceivedHandlerPython = callable; + //callable->ob_refcnt++; - return NULL; + return NULL; - //return Py_None; + //return Py_None; } #endif -} -} -} +} // namespace uart +} // namespace protocol +} // namespace vn diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/port.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/port.cpp index 0fca2d1d..76c819c3 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/port.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/port.cpp @@ -1,9 +1,11 @@ #include "vn/port.h" -namespace vn { -namespace xplat { +namespace vn +{ +namespace xplat +{ -IPort::~IPort() { } +IPort::~IPort() {} -} -} +} // namespace xplat +} // namespace vn diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/position.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/position.cpp index b42bcaf6..733ba434 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/position.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/position.cpp @@ -2,23 +2,16 @@ using namespace std; -namespace vn { -namespace math { +namespace vn +{ +namespace math +{ -PositionD::PositionD(PositionType type, vec3d pos) : - _underlyingType(type), - _data(pos) -{ } +PositionD::PositionD(PositionType type, vec3d pos) : _underlyingType(type), _data(pos) {} -PositionD PositionD::fromLla(vec3d lla) -{ - return PositionD(POS_LLA, lla); -} +PositionD PositionD::fromLla(vec3d lla) { return PositionD(POS_LLA, lla); } -PositionD PositionD::fromEcef(vec3d ecef) -{ - return PositionD(POS_ECEF, ecef); -} +PositionD PositionD::fromEcef(vec3d ecef) { return PositionD(POS_ECEF, ecef); } -} -} +} // namespace math +} // namespace vn diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/rtcmlistener.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/rtcmlistener.cpp index edd295bf..b7bc8150 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/rtcmlistener.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/rtcmlistener.cpp @@ -1,273 +1,239 @@ #include "vn/rtcmlistener.h" + #include "vn/thread.h" #ifdef _WIN32 -#pragma comment (lib, "Ws2_32.lib") +#pragma comment(lib, "Ws2_32.lib") #endif -namespace vn { - namespace xplat { - - static void HandleIncomingRTCMPackets(void* data) - { - ((rtcmlistener*)data)->communicationThreadWithRTCMServer(); - } - - - rtcmlistener::rtcmlistener() - { - } - - - bool rtcmlistener::connectToServer() - { - // Start worker thread - if (!serverThread) - { - //serverThread = std::unique_ptr(new std::thread(&RTCM_Server::CommunicationThreadWithRTCMServer, this)); - serverThread = Thread::startNew(HandleIncomingRTCMPackets, this); - } - - return true; - } - - - bool rtcmlistener::disconnectFromServer() - { - bool result = false; - - timeToQuit = true; - Thread::sleepSec(1); - serverThread->join(); - serverThread = nullptr; - - printf("RTCM_Server::CommunicationThreadWithRTCMServer: Server Thread closed.\n"); - return result; - } - - void rtcmlistener::communicationThreadWithRTCMServer() - { - // Initialize socket - int iResult = sockInit(); - if (iResult != NO_ERROR) - { - printf("RTCM_Server::CommunicationThreadWithRTCMServer: sockInit failed with error: %d\n", iResult); - return; - } - - struct addrinfo* result = NULL; - struct addrinfo* ptr = NULL; - struct addrinfo hints; - - memset(&hints, 0, sizeof(hints)); - hints.ai_family = AF_INET; - hints.ai_socktype = SOCK_STREAM; - hints.ai_protocol = IPPROTO_IP; - - iResult = getaddrinfo(serverHost.c_str(), serverPort.c_str(), &hints, &result); - if (iResult != 0) - { - printf("RTCM_Server::CommunicationThreadWithRTCMServer: getaddrinfo failed with error: %d\n", iResult); - sockQuit(); - return; - } - - // Attempt to connect to an address until one succeeds - for (ptr = result; ptr != NULL; ptr = ptr->ai_next) - { - // create a socket for connecting to server - serverSocket = socket(ptr->ai_family, ptr->ai_socktype, ptr->ai_protocol); - if (!sockValid(serverSocket)) - { - printf("RTCM_Server::CommunicationThreadWithRTCMServer: socket failed with error: %d\n", GETSOCKETERROR()); - sockQuit(); - return; - } - - // connect to server. - iResult = connect(serverSocket, ptr->ai_addr, (int)ptr->ai_addrlen); - if (iResult == SOCKET_ERROR) - { - sockClose(serverSocket); - serverSocket = INVALID_SOCKET; - continue; - } - break; - } - - freeaddrinfo(result); - - if (!sockValid(serverSocket)) - { - printf("RTCM_Server::CommunicationThreadWithRTCMServer: Unable to connect to server!\n"); - sockQuit(); - return; - } - - int bytesReceived = 0; - do { +namespace vn +{ +namespace xplat +{ + +static void HandleIncomingRTCMPackets(void * data) +{ + ((rtcmlistener *)data)->communicationThreadWithRTCMServer(); +} + +rtcmlistener::rtcmlistener() {} + +bool rtcmlistener::connectToServer() +{ + // Start worker thread + if (!serverThread) { + //serverThread = std::unique_ptr(new std::thread(&RTCM_Server::CommunicationThreadWithRTCMServer, this)); + serverThread = Thread::startNew(HandleIncomingRTCMPackets, this); + } + + return true; +} + +bool rtcmlistener::disconnectFromServer() +{ + bool result = false; + + timeToQuit = true; + Thread::sleepSec(1); + serverThread->join(); + serverThread = nullptr; + + printf("RTCM_Server::CommunicationThreadWithRTCMServer: Server Thread closed.\n"); + return result; +} + +void rtcmlistener::communicationThreadWithRTCMServer() +{ + // Initialize socket + int iResult = sockInit(); + if (iResult != NO_ERROR) { + printf( + "RTCM_Server::CommunicationThreadWithRTCMServer: sockInit failed with error: %d\n", iResult); + return; + } + + struct addrinfo * result = NULL; + struct addrinfo * ptr = NULL; + struct addrinfo hints; + + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_INET; + hints.ai_socktype = SOCK_STREAM; + hints.ai_protocol = IPPROTO_IP; + + iResult = getaddrinfo(serverHost.c_str(), serverPort.c_str(), &hints, &result); + if (iResult != 0) { + printf( + "RTCM_Server::CommunicationThreadWithRTCMServer: getaddrinfo failed with error: %d\n", + iResult); + sockQuit(); + return; + } + + // Attempt to connect to an address until one succeeds + for (ptr = result; ptr != NULL; ptr = ptr->ai_next) { + // create a socket for connecting to server + serverSocket = socket(ptr->ai_family, ptr->ai_socktype, ptr->ai_protocol); + if (!sockValid(serverSocket)) { + printf( + "RTCM_Server::CommunicationThreadWithRTCMServer: socket failed with error: %d\n", + GETSOCKETERROR()); + sockQuit(); + return; + } + + // connect to server. + iResult = connect(serverSocket, ptr->ai_addr, (int)ptr->ai_addrlen); + if (iResult == SOCKET_ERROR) { + sockClose(serverSocket); + serverSocket = INVALID_SOCKET; + continue; + } + break; + } + + freeaddrinfo(result); + + if (!sockValid(serverSocket)) { + printf("RTCM_Server::CommunicationThreadWithRTCMServer: Unable to connect to server!\n"); + sockQuit(); + return; + } + + int bytesReceived = 0; + do { #define DEFAULT_BUFLEN 10240 - char buffer[DEFAULT_BUFLEN]; - int bufferLen = DEFAULT_BUFLEN; - - //printf("RTCM_Server::CommunicationThreadWithRTCMServer: Receive data from server.\n"); - bytesReceived = recv(serverSocket, buffer, bufferLen, 0); - //printBuffer(buffer, bytesReceived); - if (bytesReceived > 0) - { - int index = 0; - - // Separate out all the messages from the buffer - while (index < bytesReceived - 2) - { - // Verify packet signature (0xD3) before attempting to process - if (buffer[index] == (char)0xD3) - { - // Ignore the first 6 bits of the second byte when calculating the size - int packetSize = (((buffer[index + 1] & 0xFF) & 0x03) * 256) + (buffer[index + 2] & 0xFF) + 6; - // NOTE: Need to add some checks just in case the sync byte is the very last byte in the buffer - // May need to do another read to get the rest of the message - - // Sanity check on packetSize is not greater than remaining buffer size - if (packetSize + index > bytesReceived) - { - // Dang it, something is not right and will cause problems if we try to process this. Just skip it and keep looking. - index++; - } - else - { - rtcmmessage message = rtcmmessage(buffer, bytesReceived, index, packetSize); - // Only keep valid messages that the sensor will accept - - if (message.valid) - { - index += packetSize; - - if (message.supported) - { - //printf("RTCM_Server::CommunicationThreadWithRTCMServer: Message %d received.\n", message.id); - if (messageNotificationActive) - { - messageNotification(message); - } - } - else - { - // If this message is not supported, then just drop it. - //printf("RTCM_Server::CommunicationThreadWithRTCMServer: Message %d is not supported.\n", message.id); - } - } - else - { - // If the message is invalid, chances are we are in the middle of a packet and just need to find the start of the next message - //printf("RTCM_Server::CommunicationThreadWithRTCMServer: Invalid Message %d Found.\n", message.id); - index++; - } - } - } - else - { - // Keep working our way down the buffer until we find a message header - index++; - } - } - //printf("Finished parsing buffer.\n"); - } - else if (bytesReceived == 0) - { - //printf("Connection closed.\n"); - } - else - { - printf("recv failed with error: %d\n", GETSOCKETERROR()); - } - } while ((bytesReceived > 0) && (!timeToQuit)); - - sockClose(serverSocket); - sockQuit(); - } - - - void rtcmlistener::registerNotifications(std::function notificationHandler) - { - messageNotification = notificationHandler; - messageNotificationActive = true; - } - - void rtcmlistener::setServerHost(std::string host) - { - serverHost = host; - } - - void rtcmlistener::setServerPort(std::string port) - { - serverPort = port; - } - - - int rtcmlistener::sockInit() - { + char buffer[DEFAULT_BUFLEN]; + int bufferLen = DEFAULT_BUFLEN; + + //printf("RTCM_Server::CommunicationThreadWithRTCMServer: Receive data from server.\n"); + bytesReceived = recv(serverSocket, buffer, bufferLen, 0); + //printBuffer(buffer, bytesReceived); + if (bytesReceived > 0) { + int index = 0; + + // Separate out all the messages from the buffer + while (index < bytesReceived - 2) { + // Verify packet signature (0xD3) before attempting to process + if (buffer[index] == (char)0xD3) { + // Ignore the first 6 bits of the second byte when calculating the size + int packetSize = + (((buffer[index + 1] & 0xFF) & 0x03) * 256) + (buffer[index + 2] & 0xFF) + 6; + // NOTE: Need to add some checks just in case the sync byte is the very last byte in the buffer + // May need to do another read to get the rest of the message + + // Sanity check on packetSize is not greater than remaining buffer size + if (packetSize + index > bytesReceived) { + // Dang it, something is not right and will cause problems if we try to process this. Just skip it and keep looking. + index++; + } else { + rtcmmessage message = rtcmmessage(buffer, bytesReceived, index, packetSize); + // Only keep valid messages that the sensor will accept + + if (message.valid) { + index += packetSize; + + if (message.supported) { + //printf("RTCM_Server::CommunicationThreadWithRTCMServer: Message %d received.\n", message.id); + if (messageNotificationActive) { + messageNotification(message); + } + } else { + // If this message is not supported, then just drop it. + //printf("RTCM_Server::CommunicationThreadWithRTCMServer: Message %d is not supported.\n", message.id); + } + } else { + // If the message is invalid, chances are we are in the middle of a packet and just need to find the start of the next message + //printf("RTCM_Server::CommunicationThreadWithRTCMServer: Invalid Message %d Found.\n", message.id); + index++; + } + } + } else { + // Keep working our way down the buffer until we find a message header + index++; + } + } + //printf("Finished parsing buffer.\n"); + } else if (bytesReceived == 0) { + //printf("Connection closed.\n"); + } else { + printf("recv failed with error: %d\n", GETSOCKETERROR()); + } + } while ((bytesReceived > 0) && (!timeToQuit)); + + sockClose(serverSocket); + sockQuit(); +} + +void rtcmlistener::registerNotifications(std::function notificationHandler) +{ + messageNotification = notificationHandler; + messageNotificationActive = true; +} + +void rtcmlistener::setServerHost(std::string host) { serverHost = host; } + +void rtcmlistener::setServerPort(std::string port) { serverPort = port; } + +int rtcmlistener::sockInit() +{ #ifdef _WIN32 - WSADATA wsa_data; - return WSAStartup(MAKEWORD(1, 1), &wsa_data); + WSADATA wsa_data; + return WSAStartup(MAKEWORD(1, 1), &wsa_data); #else - return 0; + return 0; #endif - } +} - int rtcmlistener::sockQuit() - { +int rtcmlistener::sockQuit() +{ #ifdef _WIN32 - return WSACleanup(); + return WSACleanup(); #else - return 0; + return 0; #endif - } +} - int rtcmlistener::sockClose(SOCKET s) - { - int status = 0; +int rtcmlistener::sockClose(SOCKET s) +{ + int status = 0; #ifdef _WIN32 - status = shutdown(s, SD_BOTH); - if (status == 0) - { - status = closesocket(s); - } + status = shutdown(s, SD_BOTH); + if (status == 0) { + status = closesocket(s); + } #else - status = shutdown(s, SHUT_RDWR); - if (status == 0) - { - status = close(s); - } + status = shutdown(s, SHUT_RDWR); + if (status == 0) { + status = close(s); + } #endif - return status; - } + return status; +} - bool rtcmlistener::sockValid(SOCKET s) - { - bool status = false; +bool rtcmlistener::sockValid(SOCKET s) +{ + bool status = false; #ifdef _WIN32 - status = s != INVALID_SOCKET; + status = s != INVALID_SOCKET; #else - if (s > 0) - { - status = true; - } + if (s > 0) { + status = true; + } #endif - return status; - } - - void rtcmlistener::printBuffer(char* buffer, int bufferSize) - { - printf("BUFFER["); - for (int i = 0; i < bufferSize; i++) - { - printf("0x%02hhx, ", buffer[i] & 0xFF); - } - printf("]\n"); - } - } -} \ No newline at end of file + return status; +} + +void rtcmlistener::printBuffer(char * buffer, int bufferSize) +{ + printf("BUFFER["); + for (int i = 0; i < bufferSize; i++) { + printf("0x%02hhx, ", buffer[i] & 0xFF); + } + printf("]\n"); +} +} // namespace xplat +} // namespace vn \ No newline at end of file diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/rtcmmessage.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/rtcmmessage.cpp index aa723ca3..3475cc74 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/rtcmmessage.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/rtcmmessage.cpp @@ -1,247 +1,344 @@ #include "vn/rtcmmessage.h" -namespace vn { - namespace xplat { - - - rtcmmessage::rtcmmessage(char* buffer, int bufferSize, int offset, int messageSize) - { - this->valid = false; - this->supported = false; - this->id = 0; - this->crc = 0; - this->buffer = 0; - - if (messageSize <= 0) - { - return; - } - - if (offset + messageSize <= bufferSize) - { - // Allocate enough space to make a copy of the message. - this->buffer = new char[messageSize]; - - // Make a copy of the buffer for just the message data - memcpy(this->buffer, buffer + offset, messageSize); - - // Debug: Print buffer - //printBuffer(this->buffer, messageSize); - - // process the message data - processBuffer(messageSize); - } - // else remain an invalid message - } - - void rtcmmessage::processBuffer(int messageSize) - { - // The ID is made up of 24 bits "AB C0" - this->id = ((buffer[4] & 0xF0) >> 4) + ((buffer[3] & 0x0F) << 4) + (((buffer[3] & 0xF0) >> 4) * 256); - - - switch (this->id) - { - //System setup: 1005-1007, 1033 - case 1005: - case 1006: - case 1007: - case 1033: - { - this->group = "System Setup"; - this->supported = true; - break; - } - - //GPS: 1001-1004, 1074, 1075, 1077 - case 1001: - case 1002: - case 1003: - case 1004: - case 1074: - case 1075: - case 1077: - { - this->group = "GPS"; - this->supported = true; - break; - } - - //GLONASS: 1009-1012, 1084, 1085, 1087, 1230 - case 1009: - case 1010: - case 1011: - case 1012: - case 1084: - case 1085: - case 1087: - case 1230: - { - this->group = "GLOSNASS"; - this->supported = true; - break; - } - - //Galileo: 1094, 1095, 1097 - case 1094: - case 1095: - case 1097: - { - this->group = "Galileo"; - this->supported = true; - break; - } - - //Beidou: 1124, 1125, 1127 - case 1124: - case 1125: - case 1127: - { - this->group = "Beidou"; - this->supported = true; - break; - } - - //uBlox proprietary message - case 4072: - { - this->group = "uBlox"; - this->supported = true; - break; - } - - default: - { - this->group = "Unknown"; - this->supported = false; - break; - } - } - - - // The CRC is the last 24 bits - this->crc = (unsigned long)((buffer[messageSize - 1] & 0xFF) + ((buffer[messageSize - 2] & 0xFF) << 8) + (((buffer[messageSize - 3] & 0xFF) << 16))); - - // Calculate the CRC24Q value for the buffer (excluding the 24-bit CRC at the end of the buffer) - unsigned long crc = 0; - for (int i = 0; i < messageSize - 3; i++) - { - crc = ((crc << 8) & 0xFFFFFF) ^ crc24qtab[(crc >> 16) ^ (buffer[i] & 0x0000FF)]; - } - - // Record the validity of this message - this->valid = (crc == this->crc); - - if (!this->valid) - { - printf("RTCM_Message::ProcessBuffer: CRC Mismatch for ID(%i) Group(%s): crc[%03x] calculated crc[%03x]\n", this->id, this->group.c_str(), (unsigned int)this->crc, (unsigned int)crc); - } - } - - void rtcmmessage::printBuffer(char* buffer, int bufferSize) - { - printf("Buffer: "); - for (int i = 0; i < bufferSize; i++) - { - printf("0x%02x, ", buffer[i] & 0x0000FF); - } - printf("\n"); - } - - bool rtcmmessage::unitTest() - { - char buffer[] = { - // 1005 - System Setup (19) - (char)0xd3, (char)0x00, (char)0x13, - (char)0x3e, (char)0xd0, (char)0x00, (char)0x03, (char)0xbe, (char)0x8a, (char)0xf9, (char)0x06, (char)0xab, (char)0xb3, (char)0x9a, (char)0x5d, (char)0x2a, (char)0x3e, (char)0x88, (char)0x04, (char)0xc7, (char)0x77, (char)0x09, - (char)0x6a, (char)0x44, (char)0xa5, - - // 1077 - GPS (294) - (char)0xd3, (char)0x01, (char)0x26, - (char)0x43, (char)0x50, (char)0x00, (char)0x27, (char)0x66, (char)0x6d, (char)0x02, (char)0x00, (char)0x00, (char)0x41, (char)0xc6, (char)0x54, (char)0x0a, (char)0x00, (char)0x00, (char)0x00, (char)0x00, (char)0x20, (char)0x20, (char)0x00, (char)0x00, (char)0x7f, (char)0xff, (char)0xfe, (char)0x94, (char)0x88, (char)0x96, (char)0xa0, (char)0x9e, (char)0x90, (char)0x96, (char)0xa8, (char)0x96, (char)0x92, (char)0x8a, (char)0x00, (char)0x00, (char)0x00, (char)0x00, (char)0x00, (char)0x17, (char)0x2a, (char)0xf8, (char)0x5d, (char)0xd4, (char)0x54, (char)0x48, (char)0xa7, (char)0xab, (char)0x50, (char)0x37, (char)0xdb, (char)0x5e, (char)0xae, (char)0x7c, (char)0x82, (char)0x03, (char)0x30, (char)0x47, (char)0x61, (char)0x4e, (char)0xfc, (char)0x87, (char)0xf8, (char)0xbf, (char)0xba, (char)0x5e, (char)0x9e, (char)0xff, (char)0xdb, (char)0xf9, (char)0xef, (char)0xdd, (char)0xe3, (char)0x63, (char)0x34, (char)0x38, (char)0xde, (char)0xa6, (char)0xdb, (char)0xd8, (char)0x6d, (char)0x0d, (char)0x04, (char)0x7b, (char)0x66, (char)0x49, (char)0xb7, (char)0x06, (char)0x5e, (char)0xf6, (char)0x68, (char)0x0b, (char)0x3d, (char)0x8b, (char)0x35, (char)0xd9, (char)0x0b, (char)0xc3, (char)0xb1, (char)0x88, (char)0x3b, (char)0x1f, (char)0x9f, (char)0x1e, (char)0xe3, (char)0xf2, (char)0x49, (char)0x61, (char)0x95, (char)0xfe, (char)0x1b, (char)0x1f, (char)0xbe, (char)0xd9, (char)0x5b, (char)0xed, (char)0x32, (char)0x59, (char)0x4c, (char)0xa3, (char)0x93, (char)0xc6, (char)0xe4, (char)0x47, (char)0xca, (char)0x45, (char)0xf0, (char)0x00, (char)0xf8, (char)0x18, (char)0xa1, (char)0x13, (char)0x56, (char)0x01, (char)0xd3, (char)0xb1, (char)0xa1, (char)0xd0, (char)0x6f, (char)0x21, (char)0x50, (char)0x70, (char)0x81, (char)0x75, (char)0x3b, (char)0x21, (char)0x95, (char)0xa3, (char)0x01, (char)0x9c, (char)0x2e, (char)0x5f, (char)0x6b, (char)0x7b, (char)0x5f, (char)0x75, (char)0xf9, (char)0x61, (char)0x1a, (char)0x7e, (char)0xa1, (char)0x39, (char)0x98, (char)0xbf, (char)0xe9, (char)0xb7, (char)0x9f, (char)0xf6, (char)0x7a, (char)0x80, (char)0x6d, (char)0x08, (char)0xe0, (char)0x74, (char)0xae, (char)0x5f, (char)0xd4, (char)0xdf, (char)0x1f, (char)0xe6, (char)0x0d, (char)0x5e, (char)0x72, (char)0x53, (char)0xde, (char)0x8d, (char)0x93, (char)0x81, (char)0x2d, (char)0xc0, (char)0xa1, (char)0x45, (char)0x51, (char)0x52, (char)0x74, (char)0x9b, (char)0x41, (char)0xd0, (char)0x53, (char)0xf4, (char)0xfd, (char)0x43, (char)0xd0, (char)0xf1, (char)0x8c, (char)0x61, (char)0x32, (char)0xcc, (char)0xb1, (char)0xf4, (char)0x7c, (char)0xf7, (char)0x3d, (char)0xb2, (char)0xac, (char)0xab, (char)0x30, (char)0xcc, (char)0x13, (char)0x5c, (char)0xd6, (char)0x00, (char)0x00, (char)0x05, (char)0xb3, (char)0x15, (char)0x62, (char)0x36, (char)0x0d, (char)0x97, (char)0x75, (char)0x56, (char)0x50, (char)0xb5, (char)0x92, (char)0xf3, (char)0x5e, (char)0x13, (char)0x0d, (char)0xad, (char)0x15, (char)0xcd, (char)0xca, (char)0x85, (char)0xb1, (char)0x0d, (char)0xdc, (char)0x71, (char)0x56, (char)0x41, (char)0x58, (char)0x7f, (char)0x6b, (char)0xfe, (char)0xe0, (char)0x25, (char)0xb4, (char)0x4b, (char)0x76, (char)0xe2, (char)0x4d, (char)0xc4, (char)0x7f, (char)0x75, (char)0x7e, (char)0xed, (char)0x6e, (char)0xe6, (char)0xdd, (char)0xe9, (char)0xe0, (char)0xfb, (char)0xc2, (char)0x2f, (char)0x6a, (char)0x0e, (char)0xd6, (char)0xc3, (char)0xbd, (char)0x87, (char)0x6c, (char)0x8b, (char)0x12, (char)0x16, (char)0x36, (char)0x01, (char)0x64, (char)0x02, (char)0xcf, (char)0xd2, (char)0x5f, (char)0xa5, (char)0xe0, - (char)0xa0, (char)0x24, (char)0xf2, - - // 1087 - GLOSNASS (185) - (char)0xd3, (char)0x00, (char)0xb9, - (char)0x43, (char)0xf0, (char)0x00, (char)0x40, (char)0xc5, (char)0xa1, (char)0xc2, (char)0x00, (char)0x00, (char)0x03, (char)0x01, (char)0xf0, (char)0x00, (char)0x00, (char)0x00, (char)0x00, (char)0x00, (char)0x20, (char)0x80, (char)0x00, (char)0x00, (char)0x5f, (char)0xfe, (char)0x98, (char)0x9a, (char)0x90, (char)0x86, (char)0x92, (char)0x82, (char)0x8c, (char)0x78, (char)0xed, (char)0x69, (char)0x50, (char)0x99, (char)0xc8, (char)0xfc, (char)0x58, (char)0x76, (char)0xcf, (char)0x7d, (char)0xa2, (char)0x04, (char)0x28, (char)0x01, (char)0x10, (char)0x26, (char)0x9f, (char)0xa2, (char)0x04, (char)0xb5, (char)0xfe, (char)0x3f, (char)0xa8, (char)0x21, (char)0x83, (char)0x9a, (char)0x5a, (char)0x83, (char)0xa6, (char)0x1f, (char)0x95, (char)0xdc, (char)0x83, (char)0xde, (char)0x0a, (char)0x16, (char)0x04, (char)0x0d, (char)0x60, (char)0x6e, (char)0x2e, (char)0x3e, (char)0x51, (char)0xe4, (char)0x39, (char)0x99, (char)0xcb, (char)0xc4, (char)0xdd, (char)0x0b, (char)0xe9, (char)0xbf, (char)0x61, (char)0x7c, (char)0x47, (char)0x66, (char)0x05, (char)0x5d, (char)0x92, (char)0x17, (char)0xa8, (char)0xd2, (char)0x1a, (char)0xfe, (char)0xb5, (char)0xf8, (char)0x4c, (char)0x9d, (char)0xfa, (char)0xcf, (char)0x9c, (char)0x03, (char)0x2e, (char)0x20, (char)0x05, (char)0xf8, (char)0xba, (char)0x11, (char)0xfa, (char)0xce, (char)0x13, (char)0x13, (char)0x11, (char)0xf5, (char)0x0f, (char)0x63, (char)0xf7, (char)0x84, (char)0x1d, (char)0xf0, (char)0xfa, (char)0x23, (char)0xf2, (char)0x95, (char)0x9d, (char)0x34, (char)0xc8, (char)0xf2, (char)0x35, (char)0x0f, (char)0x43, (char)0xcd, (char)0xf3, (char)0x7c, (char)0xdd, (char)0x37, (char)0x4c, (char)0x33, (char)0x0c, (char)0x57, (char)0x14, (char)0x00, (char)0x02, (char)0x35, (char)0x9d, (char)0x2d, (char)0x3c, (char)0x1a, (char)0xc7, (char)0xb8, (char)0x6b, (char)0x9b, (char)0xb6, (char)0xcb, (char)0xc5, (char)0xf0, (char)0x99, (char)0x4a, (char)0xa7, (char)0x08, (char)0x5c, (char)0x41, (char)0x40, (char)0x7f, (char)0x98, (char)0xab, (char)0x71, (char)0x5f, (char)0xe4, (char)0x22, (char)0x48, (char)0x8d, (char)0x71, (char)0x11, (char)0xe0, (char)0x53, (char)0xf3, (char)0x3f, (char)0xe8, (char)0x17, (char)0xf8, (char)0x4f, (char)0xe3, (char)0x00, - (char)0xcf, (char)0xce, (char)0x40, - - // 1097 - Galileo (335) - (char)0xd3, (char)0x01, (char)0x4f, - (char)0x44, (char)0x90, (char)0x00, (char)0x27, (char)0x66, (char)0x6d, (char)0x02, (char)0x00, (char)0x00, (char)0x14, (char)0x58, (char)0x00, (char)0xc1, (char)0x40, (char)0x00, (char)0x00, (char)0x00, (char)0x20, (char)0x01, (char)0x01, (char)0x00, (char)0x7f, (char)0xff, (char)0xff, (char)0xf5, (char)0x85, (char)0x65, (char)0x84, (char)0xf4, (char)0xe5, (char)0x15, (char)0x45, (char)0xa5, (char)0xc0, (char)0x00, (char)0x00, (char)0x00, (char)0x00, (char)0x58, (char)0x25, (char)0x42, (char)0x4d, (char)0xa1, (char)0x21, (char)0x9b, (char)0x78, (char)0xfa, (char)0xf9, (char)0xff, (char)0x00, (char)0x12, (char)0x06, (char)0x98, (char)0x21, (char)0x3f, (char)0x7e, (char)0xbf, (char)0xa9, (char)0x01, (char)0x47, (char)0xf1, (char)0x70, (char)0x4a, (char)0xc1, (char)0x68, (char)0xdb, (char)0x7f, (char)0x8d, (char)0xd1, (char)0x69, (char)0xdd, (char)0xc3, (char)0x0f, (char)0xac, (char)0x2a, (char)0xfc, (char)0x03, (char)0xbf, (char)0xcd, (char)0xd3, (char)0xcc, (char)0x2a, (char)0x0c, (char)0xe1, (char)0x6a, (char)0xce, (char)0xe8, (char)0xec, (char)0x68, (char)0xf5, (char)0xc6, (char)0x2a, (char)0xcc, (char)0x6e, (char)0x9c, (char)0xdc, (char)0x41, (char)0xdd, (char)0xbb, (char)0xb1, (char)0xdc, (char)0x98, (char)0xd1, (char)0x19, (char)0xf6, (char)0x11, (char)0x97, (char)0xe1, (char)0x26, (char)0x34, (char)0x07, (char)0x03, (char)0x90, (char)0x7e, (char)0x8c, (char)0x08, (char)0xca, (char)0x52, (char)0xe8, (char)0xd9, (char)0x2f, (char)0xfe, (char)0xc3, (char)0x0e, (char)0x06, (char)0x34, (char)0x05, (char)0x83, (char)0x4a, (char)0x6c, (char)0x35, (char)0xbc, (char)0x3f, (char)0x74, (char)0xdc, (char)0x3f, (char)0x7b, (char)0xfc, (char)0x4f, (char)0x80, (char)0x02, (char)0x1f, (char)0xeb, (char)0x7b, (char)0xdf, (char)0xf1, (char)0xde, (char)0xff, (char)0xf4, (char)0x65, (char)0xcf, (char)0x22, (char)0x0c, (char)0xef, (char)0x24, (char)0x56, (char)0x2f, (char)0x27, (char)0x34, (char)0x0f, (char)0x27, (char)0xdf, (char)0xaf, (char)0x31, (char)0xd4, (char)0x0f, (char)0x3d, (char)0x6c, (char)0xbf, (char)0x80, (char)0xea, (char)0x4f, (char)0x85, (char)0xc0, (char)0x4f, (char)0x8b, (char)0x7e, (char)0xd0, (char)0x54, (char)0xf1, (char)0xd0, (char)0x60, (char)0x1d, (char)0x50, (char)0x65, (char)0x7e, (char)0x10, (char)0x37, (char)0x49, (char)0x30, (char)0x45, (char)0x7e, (char)0x90, (char)0x4a, (char)0x4a, (char)0xe0, (char)0xbc, (char)0xfc, (char)0x80, (char)0xc5, (char)0x40, (char)0x40, (char)0xcb, (char)0x6f, (char)0x20, (char)0xd9, (char)0x97, (char)0xf0, (char)0xe5, (char)0x95, (char)0x80, (char)0xe8, (char)0x17, (char)0x89, (char)0x86, (char)0x61, (char)0x98, (char)0x68, (char)0x2a, (char)0x0a, (char)0x82, (char)0xa6, (char)0x29, (char)0x8a, (char)0x62, (char)0x4e, (char)0x93, (char)0xa4, (char)0xf9, (char)0x9e, (char)0x61, (char)0x98, (char)0x68, (char)0x9a, (char)0x26, (char)0x89, (char)0x9b, (char)0x66, (char)0xd9, (char)0xb6, (char)0x97, (char)0xa5, (char)0xe9, (char)0x79, (char)0xa2, (char)0x6e, (char)0x9b, (char)0x80, (char)0x00, (char)0x00, (char)0x05, (char)0x19, (char)0x96, (char)0x60, (char)0xf6, (char)0xfe, (char)0x2f, (char)0x7e, (char)0x51, (char)0x95, (char)0x9d, (char)0x21, (char)0x50, (char)0xde, (char)0xb6, (char)0xbd, (char)0x79, (char)0x76, (char)0xdc, (char)0x78, (char)0x06, (char)0x99, (char)0x94, (char)0xdc, (char)0x79, (char)0x4e, (char)0x39, (char)0x4e, (char)0x5b, (char)0xb6, (char)0x5c, (char)0x47, (char)0x5d, (char)0x59, (char)0x63, (char)0x17, (char)0x85, (char)0xf9, (char)0x0c, (char)0x45, (char)0x11, (char)0xa4, (char)0x21, (char)0xf0, (char)0x45, (char)0xa8, (char)0x69, (char)0x80, (char)0xcd, (char)0xa1, (char)0xb9, (char)0x86, (char)0xc8, (char)0x8e, (char)0x7e, (char)0x1b, (char)0x67, (char)0xb7, (char)0x4f, (char)0x6c, (char)0xbe, (char)0xd9, (char)0xc1, (char)0x36, (char)0x82, (char)0x5d, (char)0xc4, (char)0xcc, (char)0x04, (char)0x92, (char)0x0a, (char)0x22, (char)0x13, (char)0xdc, (char)0x93, (char)0xc1, (char)0x26, (char)0x82, (char)0x55, (char)0x9f, (char)0xb4, (char)0xff, (char)0xce, (char)0xff, (char)0x79, - (char)0xbe, (char)0xfb, (char)0x29, - - // 1127 - Beidou (124) - (char)0xd3, (char)0x00, (char)0x7c, - (char)0x46, (char)0x70, (char)0x00, (char)0x27, (char)0x65, (char)0x92, (char)0x40, (char)0x00, (char)0x00, (char)0x00, (char)0x00, (char)0x12, (char)0x1a, (char)0x0c, (char)0x00, (char)0x00, (char)0x00, (char)0x20, (char)0x00, (char)0x00, (char)0x00, (char)0x7f, (char)0x57, (char)0x59, (char)0x49, (char)0x54, (char)0x4c, (char)0x51, (char)0x51, (char)0x00, (char)0x00, (char)0x00, (char)0x02, (char)0x1a, (char)0xef, (char)0x81, (char)0x6b, (char)0xdc, (char)0xb2, (char)0xca, (char)0x65, (char)0x41, (char)0xa9, (char)0x02, (char)0xd4, (char)0x02, (char)0xf0, (char)0x83, (char)0x7e, (char)0x0c, (char)0xf9, (char)0x28, (char)0x16, (char)0x20, (char)0xf7, (char)0x77, (char)0xdb, (char)0x6c, (char)0x8d, (char)0x2c, (char)0x7d, (char)0xe5, (char)0xa5, (char)0x7f, (char)0xfb, (char)0x2f, (char)0x06, (char)0xd4, (char)0xbc, (char)0x3e, (char)0xa2, (char)0x03, (char)0x36, (char)0xe3, (char)0xf6, (char)0xcc, (char)0x7b, (char)0xf6, (char)0x2f, (char)0x7c, (char)0xfa, (char)0x61, (char)0x88, (char)0x01, (char)0x2d, (char)0xa5, (char)0x02, (char)0xd8, (char)0xec, (char)0xf1, (char)0x6f, (char)0x15, (char)0x9e, (char)0x25, (char)0x09, (char)0xbe, (char)0x7e, (char)0x94, (char)0xa5, (char)0x3a, (char)0x48, (char)0x05, (char)0x7f, (char)0x2a, (char)0x67, (char)0x96, (char)0x66, (char)0x67, (char)0x7a, (char)0xdf, (char)0x7d, (char)0xe3, (char)0x80, (char)0xe0, (char)0x6e, (char)0xde, (char)0xf7, (char)0xaa, (char)0x4b, (char)0x10, (char)0x0c, (char)0xf0, (char)0x40, (char)0x40, - (char)0xc2, (char)0x7a, (char)0xd4, - - // 1230 - Unknown (12) - (char)0xd3, (char)0x00, (char)0x0c, - (char)0x4c, (char)0xe0, (char)0x00, (char)0x8f, (char)0x00, (char)0x00, (char)0x00, (char)0x00, (char)0x00, (char)0x00, (char)0x00, (char)0x00, - (char)0xe0, (char)0x3f, (char)0x61 - }; - - int bufferSize = sizeof(buffer); - int packetSize = 0; - int offset = 0; - - // System Setup (packetSize = 19 + 6 = 25) - offset += packetSize; - packetSize = (((buffer[offset + 1] & 0xFF) & 0x03) * 256) + (buffer[offset + 2] & 0xFF) + 6; - rtcmmessage message_1005 = rtcmmessage(buffer, bufferSize, offset, packetSize); - if (message_1005.valid != true) - { - return false; - } - - // GPS (packetSize = 294 + 6 = 300) - offset += packetSize; - packetSize = (((buffer[offset + 1] & 0xFF) & 0x03) * 256) + (buffer[offset + 2] & 0xFF) + 6; - rtcmmessage message_1077 = rtcmmessage(buffer, bufferSize, offset, packetSize); - if (message_1077.valid != true) - { - return false; - } - - // GLOSNASS (packetSize = 185 + 6 = 191) - offset += packetSize; - packetSize = (((buffer[offset + 1] & 0xFF) & 0x03) * 256) + (buffer[offset + 2] & 0xFF) + 6; - rtcmmessage message_1087 = rtcmmessage(buffer, bufferSize, offset, packetSize); - if (message_1087.valid != true) - { - return false; - } - - // Galileo (packetSize = 335 + 6 = 341) - offset += packetSize; - packetSize = (((buffer[offset + 1] & 0xFF) & 0x03) * 256) + (buffer[offset + 2] & 0xFF) + 6; - rtcmmessage message_1097 = rtcmmessage(buffer, bufferSize, offset, packetSize); - if (message_1097.valid != true) - { - return false; - } - - // Beidou (packetSize = 124 + 6 = 130) - offset += packetSize; - packetSize = (((buffer[offset + 1] & 0xFF) & 0x03) * 256) + (buffer[offset + 2] & 0xFF) + 6; - rtcmmessage message_1127 = rtcmmessage(buffer, bufferSize, offset, packetSize); - if (message_1127.valid != true) - { - return false; - } - - // Unknown (packetSize = 12 + 6 = 18) - offset += packetSize; - packetSize = (((buffer[offset + 1] & 0xFF) & 0x03) * 256) + (buffer[offset + 2] & 0xFF) + 6; - rtcmmessage message_1230 = rtcmmessage(buffer, bufferSize, offset, packetSize); - if (message_1230.valid != true) - { - return false; - } - - return true; - } +namespace vn +{ +namespace xplat +{ + +rtcmmessage::rtcmmessage(char * buffer, int bufferSize, int offset, int messageSize) +{ + this->valid = false; + this->supported = false; + this->id = 0; + this->crc = 0; + this->buffer = 0; + + if (messageSize <= 0) { + return; + } + + if (offset + messageSize <= bufferSize) { + // Allocate enough space to make a copy of the message. + this->buffer = new char[messageSize]; + + // Make a copy of the buffer for just the message data + memcpy(this->buffer, buffer + offset, messageSize); + + // Debug: Print buffer + //printBuffer(this->buffer, messageSize); + + // process the message data + processBuffer(messageSize); + } + // else remain an invalid message +} + +void rtcmmessage::processBuffer(int messageSize) +{ + // The ID is made up of 24 bits "AB C0" + this->id = + ((buffer[4] & 0xF0) >> 4) + ((buffer[3] & 0x0F) << 4) + (((buffer[3] & 0xF0) >> 4) * 256); + + switch (this->id) { + //System setup: 1005-1007, 1033 + case 1005: + case 1006: + case 1007: + case 1033: { + this->group = "System Setup"; + this->supported = true; + break; + } + + //GPS: 1001-1004, 1074, 1075, 1077 + case 1001: + case 1002: + case 1003: + case 1004: + case 1074: + case 1075: + case 1077: { + this->group = "GPS"; + this->supported = true; + break; + } + + //GLONASS: 1009-1012, 1084, 1085, 1087, 1230 + case 1009: + case 1010: + case 1011: + case 1012: + case 1084: + case 1085: + case 1087: + case 1230: { + this->group = "GLOSNASS"; + this->supported = true; + break; + } + + //Galileo: 1094, 1095, 1097 + case 1094: + case 1095: + case 1097: { + this->group = "Galileo"; + this->supported = true; + break; + } + + //Beidou: 1124, 1125, 1127 + case 1124: + case 1125: + case 1127: { + this->group = "Beidou"; + this->supported = true; + break; + } + + //uBlox proprietary message + case 4072: { + this->group = "uBlox"; + this->supported = true; + break; + } + default: { + this->group = "Unknown"; + this->supported = false; + break; } -} \ No newline at end of file + } + + // The CRC is the last 24 bits + // clang-format off + this->crc = + (unsigned long)((buffer[messageSize - 1] & 0xFF) + ((buffer[messageSize - 2] & 0xFF) << 8) + + (((buffer[messageSize - 3] & 0xFF) << 16))); + // clang-format on + // Calculate the CRC24Q value for the buffer (excluding the 24-bit CRC at the end of the buffer) + unsigned long crc = 0; + for (int i = 0; i < messageSize - 3; i++) { + crc = ((crc << 8) & 0xFFFFFF) ^ crc24qtab[(crc >> 16) ^ (buffer[i] & 0x0000FF)]; + } + + // Record the validity of this message + this->valid = (crc == this->crc); + + if (!this->valid) { + printf( + "RTCM_Message::ProcessBuffer: CRC Mismatch for ID(%i) Group(%s): crc[%03x] calculated " + "crc[%03x]\n", + this->id, this->group.c_str(), (unsigned int)this->crc, (unsigned int)crc); + } +} + +void rtcmmessage::printBuffer(char * buffer, int bufferSize) +{ + printf("Buffer: "); + for (int i = 0; i < bufferSize; i++) { + printf("0x%02x, ", buffer[i] & 0x0000FF); + } + printf("\n"); +} + +bool rtcmmessage::unitTest() +{ + char buffer[] = { + // 1005 - System Setup (19) + (char)0xd3, (char)0x00, (char)0x13, (char)0x3e, (char)0xd0, (char)0x00, (char)0x03, (char)0xbe, + (char)0x8a, (char)0xf9, (char)0x06, (char)0xab, (char)0xb3, (char)0x9a, (char)0x5d, (char)0x2a, + (char)0x3e, (char)0x88, (char)0x04, (char)0xc7, (char)0x77, (char)0x09, (char)0x6a, (char)0x44, + (char)0xa5, + + // 1077 - GPS (294) + (char)0xd3, (char)0x01, (char)0x26, (char)0x43, (char)0x50, (char)0x00, (char)0x27, (char)0x66, + (char)0x6d, (char)0x02, (char)0x00, (char)0x00, (char)0x41, (char)0xc6, (char)0x54, (char)0x0a, + (char)0x00, (char)0x00, (char)0x00, (char)0x00, (char)0x20, (char)0x20, (char)0x00, (char)0x00, + (char)0x7f, (char)0xff, (char)0xfe, (char)0x94, (char)0x88, (char)0x96, (char)0xa0, (char)0x9e, + (char)0x90, (char)0x96, (char)0xa8, (char)0x96, (char)0x92, (char)0x8a, (char)0x00, (char)0x00, + (char)0x00, (char)0x00, (char)0x00, (char)0x17, (char)0x2a, (char)0xf8, (char)0x5d, (char)0xd4, + (char)0x54, (char)0x48, (char)0xa7, (char)0xab, (char)0x50, (char)0x37, (char)0xdb, (char)0x5e, + (char)0xae, (char)0x7c, (char)0x82, (char)0x03, (char)0x30, (char)0x47, (char)0x61, (char)0x4e, + (char)0xfc, (char)0x87, (char)0xf8, (char)0xbf, (char)0xba, (char)0x5e, (char)0x9e, (char)0xff, + (char)0xdb, (char)0xf9, (char)0xef, (char)0xdd, (char)0xe3, (char)0x63, (char)0x34, (char)0x38, + (char)0xde, (char)0xa6, (char)0xdb, (char)0xd8, (char)0x6d, (char)0x0d, (char)0x04, (char)0x7b, + (char)0x66, (char)0x49, (char)0xb7, (char)0x06, (char)0x5e, (char)0xf6, (char)0x68, (char)0x0b, + (char)0x3d, (char)0x8b, (char)0x35, (char)0xd9, (char)0x0b, (char)0xc3, (char)0xb1, (char)0x88, + (char)0x3b, (char)0x1f, (char)0x9f, (char)0x1e, (char)0xe3, (char)0xf2, (char)0x49, (char)0x61, + (char)0x95, (char)0xfe, (char)0x1b, (char)0x1f, (char)0xbe, (char)0xd9, (char)0x5b, (char)0xed, + (char)0x32, (char)0x59, (char)0x4c, (char)0xa3, (char)0x93, (char)0xc6, (char)0xe4, (char)0x47, + (char)0xca, (char)0x45, (char)0xf0, (char)0x00, (char)0xf8, (char)0x18, (char)0xa1, (char)0x13, + (char)0x56, (char)0x01, (char)0xd3, (char)0xb1, (char)0xa1, (char)0xd0, (char)0x6f, (char)0x21, + (char)0x50, (char)0x70, (char)0x81, (char)0x75, (char)0x3b, (char)0x21, (char)0x95, (char)0xa3, + (char)0x01, (char)0x9c, (char)0x2e, (char)0x5f, (char)0x6b, (char)0x7b, (char)0x5f, (char)0x75, + (char)0xf9, (char)0x61, (char)0x1a, (char)0x7e, (char)0xa1, (char)0x39, (char)0x98, (char)0xbf, + (char)0xe9, (char)0xb7, (char)0x9f, (char)0xf6, (char)0x7a, (char)0x80, (char)0x6d, (char)0x08, + (char)0xe0, (char)0x74, (char)0xae, (char)0x5f, (char)0xd4, (char)0xdf, (char)0x1f, (char)0xe6, + (char)0x0d, (char)0x5e, (char)0x72, (char)0x53, (char)0xde, (char)0x8d, (char)0x93, (char)0x81, + (char)0x2d, (char)0xc0, (char)0xa1, (char)0x45, (char)0x51, (char)0x52, (char)0x74, (char)0x9b, + (char)0x41, (char)0xd0, (char)0x53, (char)0xf4, (char)0xfd, (char)0x43, (char)0xd0, (char)0xf1, + (char)0x8c, (char)0x61, (char)0x32, (char)0xcc, (char)0xb1, (char)0xf4, (char)0x7c, (char)0xf7, + (char)0x3d, (char)0xb2, (char)0xac, (char)0xab, (char)0x30, (char)0xcc, (char)0x13, (char)0x5c, + (char)0xd6, (char)0x00, (char)0x00, (char)0x05, (char)0xb3, (char)0x15, (char)0x62, (char)0x36, + (char)0x0d, (char)0x97, (char)0x75, (char)0x56, (char)0x50, (char)0xb5, (char)0x92, (char)0xf3, + (char)0x5e, (char)0x13, (char)0x0d, (char)0xad, (char)0x15, (char)0xcd, (char)0xca, (char)0x85, + (char)0xb1, (char)0x0d, (char)0xdc, (char)0x71, (char)0x56, (char)0x41, (char)0x58, (char)0x7f, + (char)0x6b, (char)0xfe, (char)0xe0, (char)0x25, (char)0xb4, (char)0x4b, (char)0x76, (char)0xe2, + (char)0x4d, (char)0xc4, (char)0x7f, (char)0x75, (char)0x7e, (char)0xed, (char)0x6e, (char)0xe6, + (char)0xdd, (char)0xe9, (char)0xe0, (char)0xfb, (char)0xc2, (char)0x2f, (char)0x6a, (char)0x0e, + (char)0xd6, (char)0xc3, (char)0xbd, (char)0x87, (char)0x6c, (char)0x8b, (char)0x12, (char)0x16, + (char)0x36, (char)0x01, (char)0x64, (char)0x02, (char)0xcf, (char)0xd2, (char)0x5f, (char)0xa5, + (char)0xe0, (char)0xa0, (char)0x24, (char)0xf2, + + // 1087 - GLOSNASS (185) + (char)0xd3, (char)0x00, (char)0xb9, (char)0x43, (char)0xf0, (char)0x00, (char)0x40, (char)0xc5, + (char)0xa1, (char)0xc2, (char)0x00, (char)0x00, (char)0x03, (char)0x01, (char)0xf0, (char)0x00, + (char)0x00, (char)0x00, (char)0x00, (char)0x00, (char)0x20, (char)0x80, (char)0x00, (char)0x00, + (char)0x5f, (char)0xfe, (char)0x98, (char)0x9a, (char)0x90, (char)0x86, (char)0x92, (char)0x82, + (char)0x8c, (char)0x78, (char)0xed, (char)0x69, (char)0x50, (char)0x99, (char)0xc8, (char)0xfc, + (char)0x58, (char)0x76, (char)0xcf, (char)0x7d, (char)0xa2, (char)0x04, (char)0x28, (char)0x01, + (char)0x10, (char)0x26, (char)0x9f, (char)0xa2, (char)0x04, (char)0xb5, (char)0xfe, (char)0x3f, + (char)0xa8, (char)0x21, (char)0x83, (char)0x9a, (char)0x5a, (char)0x83, (char)0xa6, (char)0x1f, + (char)0x95, (char)0xdc, (char)0x83, (char)0xde, (char)0x0a, (char)0x16, (char)0x04, (char)0x0d, + (char)0x60, (char)0x6e, (char)0x2e, (char)0x3e, (char)0x51, (char)0xe4, (char)0x39, (char)0x99, + (char)0xcb, (char)0xc4, (char)0xdd, (char)0x0b, (char)0xe9, (char)0xbf, (char)0x61, (char)0x7c, + (char)0x47, (char)0x66, (char)0x05, (char)0x5d, (char)0x92, (char)0x17, (char)0xa8, (char)0xd2, + (char)0x1a, (char)0xfe, (char)0xb5, (char)0xf8, (char)0x4c, (char)0x9d, (char)0xfa, (char)0xcf, + (char)0x9c, (char)0x03, (char)0x2e, (char)0x20, (char)0x05, (char)0xf8, (char)0xba, (char)0x11, + (char)0xfa, (char)0xce, (char)0x13, (char)0x13, (char)0x11, (char)0xf5, (char)0x0f, (char)0x63, + (char)0xf7, (char)0x84, (char)0x1d, (char)0xf0, (char)0xfa, (char)0x23, (char)0xf2, (char)0x95, + (char)0x9d, (char)0x34, (char)0xc8, (char)0xf2, (char)0x35, (char)0x0f, (char)0x43, (char)0xcd, + (char)0xf3, (char)0x7c, (char)0xdd, (char)0x37, (char)0x4c, (char)0x33, (char)0x0c, (char)0x57, + (char)0x14, (char)0x00, (char)0x02, (char)0x35, (char)0x9d, (char)0x2d, (char)0x3c, (char)0x1a, + (char)0xc7, (char)0xb8, (char)0x6b, (char)0x9b, (char)0xb6, (char)0xcb, (char)0xc5, (char)0xf0, + (char)0x99, (char)0x4a, (char)0xa7, (char)0x08, (char)0x5c, (char)0x41, (char)0x40, (char)0x7f, + (char)0x98, (char)0xab, (char)0x71, (char)0x5f, (char)0xe4, (char)0x22, (char)0x48, (char)0x8d, + (char)0x71, (char)0x11, (char)0xe0, (char)0x53, (char)0xf3, (char)0x3f, (char)0xe8, (char)0x17, + (char)0xf8, (char)0x4f, (char)0xe3, (char)0x00, (char)0xcf, (char)0xce, (char)0x40, + + // 1097 - Galileo (335) + (char)0xd3, (char)0x01, (char)0x4f, (char)0x44, (char)0x90, (char)0x00, (char)0x27, (char)0x66, + (char)0x6d, (char)0x02, (char)0x00, (char)0x00, (char)0x14, (char)0x58, (char)0x00, (char)0xc1, + (char)0x40, (char)0x00, (char)0x00, (char)0x00, (char)0x20, (char)0x01, (char)0x01, (char)0x00, + (char)0x7f, (char)0xff, (char)0xff, (char)0xf5, (char)0x85, (char)0x65, (char)0x84, (char)0xf4, + (char)0xe5, (char)0x15, (char)0x45, (char)0xa5, (char)0xc0, (char)0x00, (char)0x00, (char)0x00, + (char)0x00, (char)0x58, (char)0x25, (char)0x42, (char)0x4d, (char)0xa1, (char)0x21, (char)0x9b, + (char)0x78, (char)0xfa, (char)0xf9, (char)0xff, (char)0x00, (char)0x12, (char)0x06, (char)0x98, + (char)0x21, (char)0x3f, (char)0x7e, (char)0xbf, (char)0xa9, (char)0x01, (char)0x47, (char)0xf1, + (char)0x70, (char)0x4a, (char)0xc1, (char)0x68, (char)0xdb, (char)0x7f, (char)0x8d, (char)0xd1, + (char)0x69, (char)0xdd, (char)0xc3, (char)0x0f, (char)0xac, (char)0x2a, (char)0xfc, (char)0x03, + (char)0xbf, (char)0xcd, (char)0xd3, (char)0xcc, (char)0x2a, (char)0x0c, (char)0xe1, (char)0x6a, + (char)0xce, (char)0xe8, (char)0xec, (char)0x68, (char)0xf5, (char)0xc6, (char)0x2a, (char)0xcc, + (char)0x6e, (char)0x9c, (char)0xdc, (char)0x41, (char)0xdd, (char)0xbb, (char)0xb1, (char)0xdc, + (char)0x98, (char)0xd1, (char)0x19, (char)0xf6, (char)0x11, (char)0x97, (char)0xe1, (char)0x26, + (char)0x34, (char)0x07, (char)0x03, (char)0x90, (char)0x7e, (char)0x8c, (char)0x08, (char)0xca, + (char)0x52, (char)0xe8, (char)0xd9, (char)0x2f, (char)0xfe, (char)0xc3, (char)0x0e, (char)0x06, + (char)0x34, (char)0x05, (char)0x83, (char)0x4a, (char)0x6c, (char)0x35, (char)0xbc, (char)0x3f, + (char)0x74, (char)0xdc, (char)0x3f, (char)0x7b, (char)0xfc, (char)0x4f, (char)0x80, (char)0x02, + (char)0x1f, (char)0xeb, (char)0x7b, (char)0xdf, (char)0xf1, (char)0xde, (char)0xff, (char)0xf4, + (char)0x65, (char)0xcf, (char)0x22, (char)0x0c, (char)0xef, (char)0x24, (char)0x56, (char)0x2f, + (char)0x27, (char)0x34, (char)0x0f, (char)0x27, (char)0xdf, (char)0xaf, (char)0x31, (char)0xd4, + (char)0x0f, (char)0x3d, (char)0x6c, (char)0xbf, (char)0x80, (char)0xea, (char)0x4f, (char)0x85, + (char)0xc0, (char)0x4f, (char)0x8b, (char)0x7e, (char)0xd0, (char)0x54, (char)0xf1, (char)0xd0, + (char)0x60, (char)0x1d, (char)0x50, (char)0x65, (char)0x7e, (char)0x10, (char)0x37, (char)0x49, + (char)0x30, (char)0x45, (char)0x7e, (char)0x90, (char)0x4a, (char)0x4a, (char)0xe0, (char)0xbc, + (char)0xfc, (char)0x80, (char)0xc5, (char)0x40, (char)0x40, (char)0xcb, (char)0x6f, (char)0x20, + (char)0xd9, (char)0x97, (char)0xf0, (char)0xe5, (char)0x95, (char)0x80, (char)0xe8, (char)0x17, + (char)0x89, (char)0x86, (char)0x61, (char)0x98, (char)0x68, (char)0x2a, (char)0x0a, (char)0x82, + (char)0xa6, (char)0x29, (char)0x8a, (char)0x62, (char)0x4e, (char)0x93, (char)0xa4, (char)0xf9, + (char)0x9e, (char)0x61, (char)0x98, (char)0x68, (char)0x9a, (char)0x26, (char)0x89, (char)0x9b, + (char)0x66, (char)0xd9, (char)0xb6, (char)0x97, (char)0xa5, (char)0xe9, (char)0x79, (char)0xa2, + (char)0x6e, (char)0x9b, (char)0x80, (char)0x00, (char)0x00, (char)0x05, (char)0x19, (char)0x96, + (char)0x60, (char)0xf6, (char)0xfe, (char)0x2f, (char)0x7e, (char)0x51, (char)0x95, (char)0x9d, + (char)0x21, (char)0x50, (char)0xde, (char)0xb6, (char)0xbd, (char)0x79, (char)0x76, (char)0xdc, + (char)0x78, (char)0x06, (char)0x99, (char)0x94, (char)0xdc, (char)0x79, (char)0x4e, (char)0x39, + (char)0x4e, (char)0x5b, (char)0xb6, (char)0x5c, (char)0x47, (char)0x5d, (char)0x59, (char)0x63, + (char)0x17, (char)0x85, (char)0xf9, (char)0x0c, (char)0x45, (char)0x11, (char)0xa4, (char)0x21, + (char)0xf0, (char)0x45, (char)0xa8, (char)0x69, (char)0x80, (char)0xcd, (char)0xa1, (char)0xb9, + (char)0x86, (char)0xc8, (char)0x8e, (char)0x7e, (char)0x1b, (char)0x67, (char)0xb7, (char)0x4f, + (char)0x6c, (char)0xbe, (char)0xd9, (char)0xc1, (char)0x36, (char)0x82, (char)0x5d, (char)0xc4, + (char)0xcc, (char)0x04, (char)0x92, (char)0x0a, (char)0x22, (char)0x13, (char)0xdc, (char)0x93, + (char)0xc1, (char)0x26, (char)0x82, (char)0x55, (char)0x9f, (char)0xb4, (char)0xff, (char)0xce, + (char)0xff, (char)0x79, (char)0xbe, (char)0xfb, (char)0x29, + + // 1127 - Beidou (124) + (char)0xd3, (char)0x00, (char)0x7c, (char)0x46, (char)0x70, (char)0x00, (char)0x27, (char)0x65, + (char)0x92, (char)0x40, (char)0x00, (char)0x00, (char)0x00, (char)0x00, (char)0x12, (char)0x1a, + (char)0x0c, (char)0x00, (char)0x00, (char)0x00, (char)0x20, (char)0x00, (char)0x00, (char)0x00, + (char)0x7f, (char)0x57, (char)0x59, (char)0x49, (char)0x54, (char)0x4c, (char)0x51, (char)0x51, + (char)0x00, (char)0x00, (char)0x00, (char)0x02, (char)0x1a, (char)0xef, (char)0x81, (char)0x6b, + (char)0xdc, (char)0xb2, (char)0xca, (char)0x65, (char)0x41, (char)0xa9, (char)0x02, (char)0xd4, + (char)0x02, (char)0xf0, (char)0x83, (char)0x7e, (char)0x0c, (char)0xf9, (char)0x28, (char)0x16, + (char)0x20, (char)0xf7, (char)0x77, (char)0xdb, (char)0x6c, (char)0x8d, (char)0x2c, (char)0x7d, + (char)0xe5, (char)0xa5, (char)0x7f, (char)0xfb, (char)0x2f, (char)0x06, (char)0xd4, (char)0xbc, + (char)0x3e, (char)0xa2, (char)0x03, (char)0x36, (char)0xe3, (char)0xf6, (char)0xcc, (char)0x7b, + (char)0xf6, (char)0x2f, (char)0x7c, (char)0xfa, (char)0x61, (char)0x88, (char)0x01, (char)0x2d, + (char)0xa5, (char)0x02, (char)0xd8, (char)0xec, (char)0xf1, (char)0x6f, (char)0x15, (char)0x9e, + (char)0x25, (char)0x09, (char)0xbe, (char)0x7e, (char)0x94, (char)0xa5, (char)0x3a, (char)0x48, + (char)0x05, (char)0x7f, (char)0x2a, (char)0x67, (char)0x96, (char)0x66, (char)0x67, (char)0x7a, + (char)0xdf, (char)0x7d, (char)0xe3, (char)0x80, (char)0xe0, (char)0x6e, (char)0xde, (char)0xf7, + (char)0xaa, (char)0x4b, (char)0x10, (char)0x0c, (char)0xf0, (char)0x40, (char)0x40, (char)0xc2, + (char)0x7a, (char)0xd4, + + // 1230 - Unknown (12) + (char)0xd3, (char)0x00, (char)0x0c, (char)0x4c, (char)0xe0, (char)0x00, (char)0x8f, (char)0x00, + (char)0x00, (char)0x00, (char)0x00, (char)0x00, (char)0x00, (char)0x00, (char)0x00, (char)0xe0, + (char)0x3f, (char)0x61}; + + int bufferSize = sizeof(buffer); + int packetSize = 0; + int offset = 0; + + // System Setup (packetSize = 19 + 6 = 25) + offset += packetSize; + packetSize = (((buffer[offset + 1] & 0xFF) & 0x03) * 256) + (buffer[offset + 2] & 0xFF) + 6; + rtcmmessage message_1005 = rtcmmessage(buffer, bufferSize, offset, packetSize); + if (message_1005.valid != true) { + return false; + } + + // GPS (packetSize = 294 + 6 = 300) + offset += packetSize; + packetSize = (((buffer[offset + 1] & 0xFF) & 0x03) * 256) + (buffer[offset + 2] & 0xFF) + 6; + rtcmmessage message_1077 = rtcmmessage(buffer, bufferSize, offset, packetSize); + if (message_1077.valid != true) { + return false; + } + + // GLOSNASS (packetSize = 185 + 6 = 191) + offset += packetSize; + packetSize = (((buffer[offset + 1] & 0xFF) & 0x03) * 256) + (buffer[offset + 2] & 0xFF) + 6; + rtcmmessage message_1087 = rtcmmessage(buffer, bufferSize, offset, packetSize); + if (message_1087.valid != true) { + return false; + } + + // Galileo (packetSize = 335 + 6 = 341) + offset += packetSize; + packetSize = (((buffer[offset + 1] & 0xFF) & 0x03) * 256) + (buffer[offset + 2] & 0xFF) + 6; + rtcmmessage message_1097 = rtcmmessage(buffer, bufferSize, offset, packetSize); + if (message_1097.valid != true) { + return false; + } + + // Beidou (packetSize = 124 + 6 = 130) + offset += packetSize; + packetSize = (((buffer[offset + 1] & 0xFF) & 0x03) * 256) + (buffer[offset + 2] & 0xFF) + 6; + rtcmmessage message_1127 = rtcmmessage(buffer, bufferSize, offset, packetSize); + if (message_1127.valid != true) { + return false; + } + + // Unknown (packetSize = 12 + 6 = 18) + offset += packetSize; + packetSize = (((buffer[offset + 1] & 0xFF) & 0x03) * 256) + (buffer[offset + 2] & 0xFF) + 6; + rtcmmessage message_1230 = rtcmmessage(buffer, bufferSize, offset, packetSize); + if (message_1230.valid != true) { + return false; + } + + return true; +} + +} // namespace xplat +} // namespace vn diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/searcher.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/searcher.cpp index 120430c7..8a883e1e 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/searcher.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/searcher.cpp @@ -1,207 +1,200 @@ #include "vn/searcher.h" -#include "vn/serialport.h" -#include "vn/event.h" -#include "vn/thread.h" -#include "vn/packetfinder.h" #include +#include "vn/event.h" +#include "vn/packetfinder.h" +#include "vn/serialport.h" +#include "vn/thread.h" + using namespace std; using namespace vn::xplat; using namespace vn::protocol::uart; -namespace vn { -namespace sensors { +namespace vn +{ +namespace sensors +{ struct TestHelper { - SerialPort *serialPort; - PacketFinder *packetFinder; - Event waitForCheckingOnPort; - - TestHelper(SerialPort *serialPort, PacketFinder *packetFinder) : - serialPort(serialPort), - packetFinder(packetFinder) - { } + SerialPort * serialPort; + PacketFinder * packetFinder; + Event waitForCheckingOnPort; + + TestHelper(SerialPort * serialPort, PacketFinder * packetFinder) + : serialPort(serialPort), packetFinder(packetFinder) + { + } }; struct SearchHelper { - Event finishedSearchingPort; - bool sensorFound; - string portName; - int32_t foundBaudrate; - - explicit SearchHelper(const string &portName) : - sensorFound(false), - portName(portName), - foundBaudrate(0) - { } + Event finishedSearchingPort; + bool sensorFound; + string portName; + int32_t foundBaudrate; + + explicit SearchHelper(const string & portName) + : sensorFound(false), portName(portName), foundBaudrate(0) + { + } }; -void testDataReceivedHandler(void* userData); -void testValidPacketFoundHandler(void *userData, Packet &packet, size_t runningIndexOfPacketStart, TimeStamp timestamp); -void searchThread(void* routineData); +void testDataReceivedHandler(void * userData); +void testValidPacketFoundHandler( + void * userData, Packet & packet, size_t runningIndexOfPacketStart, TimeStamp timestamp); +void searchThread(void * routineData); // Collection of baudrates to test for sensors. They are listed in order of // liklyness and fastness. #if __cplusplus >= 201103L -vector TestBaudrates { 115200, 128000, 230400, 460800, 921600, 57600, 38400, 19200, 9600 }; +vector TestBaudrates{115200, 128000, 230400, 460800, 921600, 57600, 38400, 19200, 9600}; #else -uint32_t TestBaudratesRaw[] = { 115200, 128000, 230400, 460800, 921600, 57600, 38400, 19200, 9600 }; +uint32_t TestBaudratesRaw[] = {115200, 128000, 230400, 460800, 921600, 57600, 38400, 19200, 9600}; #endif -bool Searcher::search(const string &portName, int32_t *foundBaudrate) +bool Searcher::search(const string & portName, int32_t * foundBaudrate) { - #if __cplusplus < 201103L - vector TestBaudrates(TestBaudratesRaw, TestBaudratesRaw + sizeof(TestBaudratesRaw) / sizeof(TestBaudratesRaw[0])); - #endif - for (vector::const_iterator it = TestBaudrates.begin(); it != TestBaudrates.end(); ++it) - { - uint32_t curBaudrate = *it; - - if (test(portName, curBaudrate)) - { - *foundBaudrate = curBaudrate; - - return true; - } - } - - return false; +#if __cplusplus < 201103L + vector TestBaudrates( + TestBaudratesRaw, TestBaudratesRaw + sizeof(TestBaudratesRaw) / sizeof(TestBaudratesRaw[0])); +#endif + for (vector::const_iterator it = TestBaudrates.begin(); it != TestBaudrates.end(); + ++it) { + uint32_t curBaudrate = *it; + + if (test(portName, curBaudrate)) { + *foundBaudrate = curBaudrate; + + return true; + } + } + + return false; } vector > Searcher::search() { - std::vector portsToCheck = SerialPort::getPortNames(); - return search(portsToCheck); + std::vector portsToCheck = SerialPort::getPortNames(); + return search(portsToCheck); } -vector > Searcher::search(vector& portsToCheck) +vector > Searcher::search(vector & portsToCheck) { - list helpers; - vector > result; + list helpers; + vector > result; - // Start threads to test each port. - for (vector::const_iterator it = portsToCheck.begin(); it != portsToCheck.end(); ++it) - { - SearchHelper *sh = new SearchHelper(*it); + // Start threads to test each port. + for (vector::const_iterator it = portsToCheck.begin(); it != portsToCheck.end(); ++it) { + SearchHelper * sh = new SearchHelper(*it); - helpers.push_back(sh); + helpers.push_back(sh); - Thread::startNew(searchThread, sh); - } + Thread::startNew(searchThread, sh); + } - // Wait for each thread to finish. - for (list::const_iterator it = helpers.begin(); it != helpers.end(); ++it) - { - SearchHelper* sh = (*it); + // Wait for each thread to finish. + for (list::const_iterator it = helpers.begin(); it != helpers.end(); ++it) { + SearchHelper * sh = (*it); - sh->finishedSearchingPort.wait(); + sh->finishedSearchingPort.wait(); - if (sh->sensorFound) - { - result.push_back(pair(sh->portName, sh->foundBaudrate)); - } + if (sh->sensorFound) { + result.push_back(pair(sh->portName, sh->foundBaudrate)); + } - delete sh; - } + delete sh; + } - return result; + return result; } bool Searcher::test(string portName, uint32_t baudrate) { - SerialPort sp(portName, baudrate); - PacketFinder pf; - - try - { - sp.open(); - } - catch (invalid_argument) - { - // Probably an unsupported baudrate for the port. - std::cout << baudrate << " is an invalid baudrate for " << portName << std::endl; - return false; - } - catch (exception e) - { - // Catch everythign else - return false; - } - - TestHelper th(&sp, &pf); - - pf.registerPossiblePacketFoundHandler(&th, testValidPacketFoundHandler); - sp.registerDataReceivedHandler(&th, testDataReceivedHandler); - - // Wait for a few milliseconds to see if we receive any asynchronous - // data packets. - if (th.waitForCheckingOnPort.waitMs(50) == Event::WAIT_SIGNALED) - { - sp.close(); - - return true; - } - - // We have received any asynchronous data packets so let's try sending - // some commands. - for (size_t i = 0; i < 9; i++) - { - sp.write("$VNRRG,01*XX\r\n", 14); - - if (th.waitForCheckingOnPort.waitMs(50) == Event::WAIT_SIGNALED) - { - sp.close(); - - return true; - } - } - - sp.close(); - - return false; + SerialPort sp(portName, baudrate); + PacketFinder pf; + + try { + sp.open(); + } catch (invalid_argument) { + // Probably an unsupported baudrate for the port. + std::cout << baudrate << " is an invalid baudrate for " << portName << std::endl; + return false; + } catch (exception e) { + // Catch everythign else + return false; + } + + TestHelper th(&sp, &pf); + + pf.registerPossiblePacketFoundHandler(&th, testValidPacketFoundHandler); + sp.registerDataReceivedHandler(&th, testDataReceivedHandler); + + // Wait for a few milliseconds to see if we receive any asynchronous + // data packets. + if (th.waitForCheckingOnPort.waitMs(50) == Event::WAIT_SIGNALED) { + sp.close(); + + return true; + } + + // We have received any asynchronous data packets so let's try sending + // some commands. + for (size_t i = 0; i < 9; i++) { + sp.write("$VNRRG,01*XX\r\n", 14); + + if (th.waitForCheckingOnPort.waitMs(50) == Event::WAIT_SIGNALED) { + sp.close(); + + return true; + } + } + + sp.close(); + + return false; } -void searchThread(void* routineData) +void searchThread(void * routineData) { - SearchHelper *sh = static_cast(routineData); + SearchHelper * sh = static_cast(routineData); - sh->sensorFound = Searcher::search(sh->portName, &sh->foundBaudrate); + sh->sensorFound = Searcher::search(sh->portName, &sh->foundBaudrate); - sh->finishedSearchingPort.signal(); + sh->finishedSearchingPort.signal(); } -void testDataReceivedHandler(void* userData) +void testDataReceivedHandler(void * userData) { - TestHelper *th = static_cast(userData); + TestHelper * th = static_cast(userData); - char buffer[0x100]; - size_t numOfBytesRead = 0; + char buffer[0x100]; + size_t numOfBytesRead = 0; - th->serialPort->read(buffer, 0x100, numOfBytesRead); + th->serialPort->read(buffer, 0x100, numOfBytesRead); - th->packetFinder->processReceivedData(buffer, numOfBytesRead, false); + th->packetFinder->processReceivedData(buffer, numOfBytesRead, false); } #if defined(_MSC_VER) - #pragma warning(push) +#pragma warning(push) - // Disable warnings about unused parameters. - #pragma warning(disable:4100) +// Disable warnings about unused parameters. +#pragma warning(disable : 4100) #endif -void testValidPacketFoundHandler(void *userData, Packet &packet, size_t runningIndexOfPacketStart, TimeStamp timestamp) +void testValidPacketFoundHandler( + void * userData, Packet & packet, size_t runningIndexOfPacketStart, TimeStamp timestamp) { - TestHelper *th = static_cast(userData); + TestHelper * th = static_cast(userData); - th->waitForCheckingOnPort.signal(); + th->waitForCheckingOnPort.signal(); } -#if defined (_MSC_VER) - #pragma warning(pop) +#if defined(_MSC_VER) +#pragma warning(pop) #endif -} -} +} // namespace sensors +} // namespace vn diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/sensors.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/sensors.cpp index 8e450877..69d8662a 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/sensors.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/sensors.cpp @@ -1,25 +1,26 @@ #include "vn/sensors.h" -#include "vn/serialport.h" + +#include +#include + +#include +#include + +#include "vn/compiler.h" #include "vn/criticalsection.h" -#include "vn/vntime.h" +#include "vn/error_detection.h" #include "vn/event.h" #include "vn/exceptions.h" -#include "vn/error_detection.h" -#include "vn/vector.h" #include "vn/matrix.h" -#include "vn/compiler.h" -#include "vn/util.h" +#include "vn/serialport.h" #include "vn/thread.h" - - -#include -#include -#include -#include +#include "vn/util.h" +#include "vn/vector.h" +#include "vn/vntime.h" #if PYTHON - #include "util.h" +#include "util.h" #endif using namespace std; @@ -30,800 +31,712 @@ using namespace vn::data::integrity; #define COMMAND_MAX_LENGTH 0x100 -namespace vn { -namespace sensors { +namespace vn +{ +namespace sensors +{ -sensor_error::sensor_error() -{ } +sensor_error::sensor_error() {} -sensor_error::sensor_error(SensorError e) : - exception(), - error(e), - _errorMessage(NULL) -{ } +sensor_error::sensor_error(SensorError e) : exception(), error(e), _errorMessage(NULL) {} -sensor_error::sensor_error(sensor_error const& e) : - exception(), - error(e.error), - _errorMessage(NULL) -{ } +sensor_error::sensor_error(sensor_error const & e) +: exception(), error(e.error), _errorMessage(NULL) +{ +} sensor_error::~sensor_error() throw() { - if (_errorMessage != NULL) - delete[] _errorMessage; + if (_errorMessage != NULL) delete[] _errorMessage; } -char const* sensor_error::what() const throw() +char const * sensor_error::what() const throw() { - if (_errorMessage != NULL) - return _errorMessage; + if (_errorMessage != NULL) return _errorMessage; - string errorMsg = "received sensor error '" + str(error) + "'"; + string errorMsg = "received sensor error '" + str(error) + "'"; - const_cast(_errorMessage) = new char[errorMsg.size() + 1]; + const_cast(_errorMessage) = new char[errorMsg.size() + 1]; - #if VN_HAVE_SECURE_CRT - strcpy_s(_errorMessage, errorMsg.size() + 1, errorMsg.c_str()); - #else - strcpy(_errorMessage, errorMsg.c_str()); - #endif +#if VN_HAVE_SECURE_CRT + strcpy_s(_errorMessage, errorMsg.size() + 1, errorMsg.c_str()); +#else + strcpy(_errorMessage, errorMsg.c_str()); +#endif - return _errorMessage; + return _errorMessage; } struct VnSensor::Impl { - static const size_t DefaultReadBufferSize = 256; - static const uint16_t DefaultResponseTimeoutMs = 500; - static const uint16_t DefaultRetransmitDelayMs = 200; - - SerialPort *pSerialPort; - char readBuffer[DefaultReadBufferSize]; - IPort* port; - bool SimplePortIsOurs; - bool DidWeOpenSimplePort; - RawDataReceivedHandler _rawDataReceivedHandler; - void* _rawDataReceivedUserData; - PossiblePacketFoundHandler _possiblePacketFoundHandler; - void* _possiblePacketFoundUserData; - PacketFinder _packetFinder; - size_t _dataRunningIndex; - AsyncPacketReceivedHandler _asyncPacketReceivedHandler; - void* _asyncPacketReceivedUserData; - ErrorDetectionMode _sendErrorDetectionMode; - VnSensor* BackReference; - queue _receivedResponses; - CriticalSection _transactionCS; - bool _waitingForResponse; - bool _filteringBootloaderResponses; - ErrorPacketReceivedHandler _errorPacketReceivedHandler; - void* _errorPacketReceivedUserData; - uint16_t _responseTimeoutMs; - uint16_t _retransmitDelayMs; - xplat::Event _newResponsesEvent; - #if PYTHON - PyObject* _rawDataReceivedHandlerPython; - PyObject* _asyncPacketReceivedHandlerPython; - #endif - - explicit Impl(VnSensor* backReference) : - pSerialPort(NULL), - port(NULL), - SimplePortIsOurs(false), - DidWeOpenSimplePort(false), - _rawDataReceivedHandler(NULL), - _rawDataReceivedUserData(NULL), - _possiblePacketFoundHandler(NULL), - _possiblePacketFoundUserData(NULL), - _dataRunningIndex(0), - _asyncPacketReceivedHandler(NULL), - _asyncPacketReceivedUserData(NULL), - _sendErrorDetectionMode(ERRORDETECTIONMODE_CHECKSUM), - BackReference(backReference), - _waitingForResponse(false), - _filteringBootloaderResponses(false), - _errorPacketReceivedHandler(NULL), - _errorPacketReceivedUserData(NULL), - _responseTimeoutMs(DefaultResponseTimeoutMs), - _retransmitDelayMs(DefaultRetransmitDelayMs) - #if PYTHON - , - _asyncPacketReceivedHandlerPython(NULL), - _rawDataReceivedHandlerPython(NULL) - #endif - { - _packetFinder.registerPossiblePacketFoundHandler(this, possiblePacketFoundHandler); - } - - ~Impl() - { - _packetFinder.unregisterPossiblePacketFoundHandler(); - } - - void onPossiblePacketFound(Packet& possiblePacket, size_t packetStartRunningIndex) - { - if (_possiblePacketFoundHandler != NULL) - _possiblePacketFoundHandler(_possiblePacketFoundUserData, possiblePacket, packetStartRunningIndex); - } - - void onAsyncPacketReceived(Packet& asciiPacket, size_t runningIndex, TimeStamp timestamp) - { - - if (_asyncPacketReceivedHandler != NULL) - _asyncPacketReceivedHandler(_asyncPacketReceivedUserData, asciiPacket, runningIndex); - - #if PYTHON - BackReference->eventAsyncPacketReceived.fire(asciiPacket, runningIndex, timestamp); - #endif - - //#if PYTHON - //if (_asyncPacketReceivedHandlerPython != NULL) - //{ - // python::AcquireGIL scopedLock; - // - // boost::python::call(_asyncPacketReceivedHandlerPython, boost::ref(asciiPacket), runningIndex); - //} - //#endif - } - - void onErrorPacketReceived(Packet& errorPacket, size_t runningIndex) - { - if (_errorPacketReceivedHandler != NULL) - _errorPacketReceivedHandler(_errorPacketReceivedUserData, errorPacket, runningIndex); - } - - static void possiblePacketFoundHandler(void* userData, Packet& possiblePacket, size_t packetStartRunningIndex, TimeStamp timestamp) - { - Impl* pThis = static_cast(userData); - - pThis->onPossiblePacketFound(possiblePacket, packetStartRunningIndex); - - if (!possiblePacket.isValid()) - { - return; - } - - if (pThis->_filteringBootloaderResponses && !possiblePacket.isBootloader()) - { - // If anything other than bootloader packets arrive, turn off the bootloader filter - pThis->_filteringBootloaderResponses = false; - } - - if (possiblePacket.isError()) - { - if (pThis->_waitingForResponse) - { - pThis->_transactionCS.enter(); - pThis->_receivedResponses.push(possiblePacket); - pThis->_newResponsesEvent.signal(); - pThis->_transactionCS.leave(); - } - - pThis->onErrorPacketReceived(possiblePacket, packetStartRunningIndex); - - return; - } - - if (possiblePacket.isResponse()) - { - if (pThis->_waitingForResponse) - { - pThis->_transactionCS.enter(); - pThis->_receivedResponses.push(possiblePacket); - pThis->_newResponsesEvent.signal(); - pThis->_transactionCS.leave(); - } - - return; - } - - // This wasn't anything else. We assume it is an async packet. - pThis->onAsyncPacketReceived(possiblePacket, packetStartRunningIndex, timestamp); - } - - static void dataReceivedHandler(void* userData) - { - Impl *pi = static_cast(userData); - - size_t numOfBytesRead = 0; - - pi->port->read( - pi->readBuffer, - DefaultReadBufferSize, - numOfBytesRead); - - if (numOfBytesRead == 0) - return; - - TimeStamp t = TimeStamp::get(); - - if (pi->_rawDataReceivedHandler != NULL) - pi->_rawDataReceivedHandler(pi->_rawDataReceivedUserData, reinterpret_cast(pi->readBuffer), numOfBytesRead, pi->_dataRunningIndex); - - #if PYTHON - if (pi->_rawDataReceivedHandlerPython != NULL) - { - vector pRawData(readBuffer, readBuffer + numOfBytesRead); - - python::AcquireGIL scopedLock; - - boost::python::call(pi->_rawDataReceivedHandlerPython, pRawData, pi->_dataRunningIndex); - } - #endif - - pi->_packetFinder.processReceivedData(reinterpret_cast(pi->readBuffer), numOfBytesRead, pi->_filteringBootloaderResponses, t); - - pi->_dataRunningIndex += numOfBytesRead; - } - - bool isConnected() - { - return port != NULL && port->isOpen(); - } - - size_t finalizeCommandToSend(char *toSend, size_t length) - { - #if defined(_MSC_VER) - // Unable to use save version 'sprintf_s' since the length of 'toSend' is unknown here. - #pragma warning(push) - #pragma warning(disable:4996) - #endif - - if (_sendErrorDetectionMode == ERRORDETECTIONMODE_CHECKSUM) - { - length += sprintf(toSend + length, "%02X\r\n", Checksum8::compute(toSend + 1, length - 2)); - } - else if (_sendErrorDetectionMode == ERRORDETECTIONMODE_CRC) - { - length += sprintf(toSend + length, "%04X\r\n", Crc16::compute(toSend + 1, length - 2)); - } - else - { - length += sprintf(toSend + length, "XX\r\n"); - } - - #if defined(_MSC_VER) - #pragma warning(pop) - #endif - - return length; - } - - Packet transactionWithWait(char* toSend, size_t length, uint16_t responseTimeoutMs, uint16_t retransmitDelayMs) - { - // Make sure we don't have any existing responses. - _transactionCS.enter(); - - #if VN_SUPPORTS_SWAP - queue empty; - _receivedResponses.swap(empty); - #else - while (!_receivedResponses.empty()) _receivedResponses.pop(); - #endif - _waitingForResponse = true; - _transactionCS.leave(); - - // Send the command and continue sending if retransmits are enabled - // until we receive the response or timeout. - Stopwatch timeoutSw; - - port->write(toSend, length); - float curElapsedTime = timeoutSw.elapsedMs(); - - while (true) - { - bool shouldRetransmit = false; - - // Compute how long we should wait for a response before taking - // more action. - float responseWaitTime = responseTimeoutMs - curElapsedTime; - if (responseWaitTime > retransmitDelayMs) - { - responseWaitTime = retransmitDelayMs; - shouldRetransmit = true; - } - - // See if we have time left. - if (responseWaitTime < 0) - { - _waitingForResponse = false; - throw timeout(); - } - - // Wait for any new responses that come in or until it is time to - // send a new retransmit. - xplat::Event::WaitResult waitResult = _newResponsesEvent.waitUs(static_cast(responseWaitTime * 1000)); - - queue responsesToProcess; - - if (waitResult == xplat::Event::WAIT_TIMEDOUT) - { - if (!shouldRetransmit) - { - _waitingForResponse = false; - throw timeout(); - } - } - - if (waitResult == xplat::Event::WAIT_SIGNALED) - { - // Get the current collection of responses. - _transactionCS.enter(); - #if VN_SUPPORTS_SWAP - _receivedResponses.swap(responsesToProcess); - #else - while (!_receivedResponses.empty()) - { - responsesToProcess.push(_receivedResponses.front()); - _receivedResponses.pop(); - } - #endif - _transactionCS.leave(); - } - - // Process the collection of responses we have. - while (!responsesToProcess.empty()) - { - Packet p = responsesToProcess.front(); - responsesToProcess.pop(); - - if (p.isError()) - { - _waitingForResponse = false; - throw sensor_error(p.parseError()); - } - - // We must have a response packet. - _waitingForResponse = false; - Thread::sleepMs(1); - return p; - } - - // Retransmit. - port->write(toSend, length); - curElapsedTime = timeoutSw.elapsedMs(); - } - } - - void transactionNoFinalize(char* toSend, size_t length, bool waitForReply, Packet *response, uint16_t responseTimeoutMs, uint16_t retransmitDelayMs) - { - if (!isConnected()) - throw invalid_operation(); - - if (waitForReply) - { - *response = transactionWithWait(toSend, length, responseTimeoutMs, retransmitDelayMs); - - if (response->isError()) - throw sensor_error(response->parseError()); - } - else - { - port->write(toSend, length); - } - } - - void transactionNoFinalize(char* toSend, size_t length, bool waitForReply, Packet *response) - { - transactionNoFinalize(toSend, length, waitForReply, response, _responseTimeoutMs, _retransmitDelayMs); - } - - void transaction(char* toSend, size_t length, bool waitForReply, Packet *response, uint16_t responseTimeoutMs, uint16_t retransmitDelayMs) - { - length = finalizeCommandToSend(toSend, length); - - transactionNoFinalize(toSend, length, waitForReply, response, responseTimeoutMs, retransmitDelayMs); - } - - // Performs a communication transaction with the sensor. If waitForReply is - // set to true, we will retransmit the message until we receive a response - // or until we time out, depending on current settings. - void transaction(char* toSend, size_t length, bool waitForReply, Packet *response) - { - transaction(toSend, length, waitForReply, response, _responseTimeoutMs, _retransmitDelayMs); - } - - BinaryOutputRegister readBinaryOutput(uint8_t binaryOutputNumber) - { - char toSend[17]; - Packet response; - uint16_t asyncMode, rateDivisor, outputGroup, commonField, timeField, imuField, gpsField, attitudeField, insField, gps2Field; - - #if VN_HAVE_SECURE_CRT - int length = sprintf_s(toSend, sizeof(toSend), "$VNRRG,%u*", 74 + binaryOutputNumber); - #else - int length = sprintf(toSend, "$VNRRG,%u*", 74 + binaryOutputNumber); - #endif - - transaction(toSend, length, true, &response); - - response.parseBinaryOutput( - &asyncMode, - &rateDivisor, - &outputGroup, - &commonField, - &timeField, - &imuField, - &gpsField, - &attitudeField, - &insField, - &gps2Field); - - return BinaryOutputRegister( - static_cast(asyncMode), - rateDivisor, - static_cast(commonField), - static_cast(timeField), - static_cast(imuField), - static_cast(gpsField), - static_cast(attitudeField), - static_cast(insField), - static_cast(gps2Field)); - } - - void writeBinaryOutput(uint8_t binaryOutputNumber, BinaryOutputRegister &fields, bool waitForReply) - { - char toSend[256]; - Packet response; - - // First determine which groups are present. - uint16_t groups = 0; - if (fields.commonField) - groups |= 0x0001; - if (fields.timeField) - groups |= 0x0002; - if (fields.imuField) - groups |= 0x0004; - if (fields.gpsField) - groups |= 0x0008; - if (fields.attitudeField) - groups |= 0x0010; - if (fields.insField) - groups |= 0x0020; - if(fields.gps2Field) - groups |= 0x0040; - - #if VN_HAVE_SECURE_CRT - int length = sprintf_s(toSend, sizeof(toSend), "$VNWRG,%u,%u,%u,%X", 74 + binaryOutputNumber, fields.asyncMode, fields.rateDivisor, groups); - #else - int length = sprintf(toSend, "$VNWRG,%u,%u,%u,%X", 74 + binaryOutputNumber, fields.asyncMode, fields.rateDivisor, groups); - #endif - - if (fields.commonField) - #if VN_HAVE_SECURE_CRT - length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.commonField); - #else - length += sprintf(toSend + length, ",%X", fields.commonField); - #endif - if (fields.timeField) - #if VN_HAVE_SECURE_CRT - length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.timeField); - #else - length += sprintf(toSend + length, ",%X", fields.timeField); - #endif - if (fields.imuField) - #if VN_HAVE_SECURE_CRT - length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.imuField); - #else - length += sprintf(toSend + length, ",%X", fields.imuField); - #endif - if (fields.gpsField) - #if VN_HAVE_SECURE_CRT - length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.gpsField); - #else - length += sprintf(toSend + length, ",%X", fields.gpsField); - #endif - if (fields.attitudeField) - #if VN_HAVE_SECURE_CRT - length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.attitudeField); - #else - length += sprintf(toSend + length, ",%X", fields.attitudeField); - #endif - if (fields.insField) - #if VN_HAVE_SECURE_CRT - length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.insField); - #else - length += sprintf(toSend + length, ",%X", fields.insField); - #endif - if(fields.gps2Field) - #if VN_HAVE_SECURE_CRT + static const size_t DefaultReadBufferSize = 256; + static const uint16_t DefaultResponseTimeoutMs = 500; + static const uint16_t DefaultRetransmitDelayMs = 200; + + SerialPort * pSerialPort; + char readBuffer[DefaultReadBufferSize]; + IPort * port; + bool SimplePortIsOurs; + bool DidWeOpenSimplePort; + RawDataReceivedHandler _rawDataReceivedHandler; + void * _rawDataReceivedUserData; + PossiblePacketFoundHandler _possiblePacketFoundHandler; + void * _possiblePacketFoundUserData; + PacketFinder _packetFinder; + size_t _dataRunningIndex; + AsyncPacketReceivedHandler _asyncPacketReceivedHandler; + void * _asyncPacketReceivedUserData; + ErrorDetectionMode _sendErrorDetectionMode; + VnSensor * BackReference; + queue _receivedResponses; + CriticalSection _transactionCS; + bool _waitingForResponse; + bool _filteringBootloaderResponses; + ErrorPacketReceivedHandler _errorPacketReceivedHandler; + void * _errorPacketReceivedUserData; + uint16_t _responseTimeoutMs; + uint16_t _retransmitDelayMs; + xplat::Event _newResponsesEvent; +#if PYTHON + PyObject * _rawDataReceivedHandlerPython; + PyObject * _asyncPacketReceivedHandlerPython; +#endif + + explicit Impl(VnSensor * backReference) + : pSerialPort(NULL), + port(NULL), + SimplePortIsOurs(false), + DidWeOpenSimplePort(false), + _rawDataReceivedHandler(NULL), + _rawDataReceivedUserData(NULL), + _possiblePacketFoundHandler(NULL), + _possiblePacketFoundUserData(NULL), + _dataRunningIndex(0), + _asyncPacketReceivedHandler(NULL), + _asyncPacketReceivedUserData(NULL), + _sendErrorDetectionMode(ERRORDETECTIONMODE_CHECKSUM), + BackReference(backReference), + _waitingForResponse(false), + _filteringBootloaderResponses(false), + _errorPacketReceivedHandler(NULL), + _errorPacketReceivedUserData(NULL), + _responseTimeoutMs(DefaultResponseTimeoutMs), + _retransmitDelayMs(DefaultRetransmitDelayMs) +#if PYTHON + , + _asyncPacketReceivedHandlerPython(NULL), + _rawDataReceivedHandlerPython(NULL) +#endif + { + _packetFinder.registerPossiblePacketFoundHandler(this, possiblePacketFoundHandler); + } + + ~Impl() { _packetFinder.unregisterPossiblePacketFoundHandler(); } + + void onPossiblePacketFound(Packet & possiblePacket, size_t packetStartRunningIndex) + { + if (_possiblePacketFoundHandler != NULL) + _possiblePacketFoundHandler( + _possiblePacketFoundUserData, possiblePacket, packetStartRunningIndex); + } + + void onAsyncPacketReceived(Packet & asciiPacket, size_t runningIndex, TimeStamp timestamp) + { + if (_asyncPacketReceivedHandler != NULL) + _asyncPacketReceivedHandler(_asyncPacketReceivedUserData, asciiPacket, runningIndex); + +#if PYTHON + BackReference->eventAsyncPacketReceived.fire(asciiPacket, runningIndex, timestamp); +#endif + + //#if PYTHON + //if (_asyncPacketReceivedHandlerPython != NULL) + //{ + // python::AcquireGIL scopedLock; + // + // boost::python::call(_asyncPacketReceivedHandlerPython, boost::ref(asciiPacket), runningIndex); + //} + //#endif + } + + void onErrorPacketReceived(Packet & errorPacket, size_t runningIndex) + { + if (_errorPacketReceivedHandler != NULL) + _errorPacketReceivedHandler(_errorPacketReceivedUserData, errorPacket, runningIndex); + } + + static void possiblePacketFoundHandler( + void * userData, Packet & possiblePacket, size_t packetStartRunningIndex, TimeStamp timestamp) + { + Impl * pThis = static_cast(userData); + + pThis->onPossiblePacketFound(possiblePacket, packetStartRunningIndex); + + if (!possiblePacket.isValid()) { + return; + } + + if (pThis->_filteringBootloaderResponses && !possiblePacket.isBootloader()) { + // If anything other than bootloader packets arrive, turn off the bootloader filter + pThis->_filteringBootloaderResponses = false; + } + + if (possiblePacket.isError()) { + if (pThis->_waitingForResponse) { + pThis->_transactionCS.enter(); + pThis->_receivedResponses.push(possiblePacket); + pThis->_newResponsesEvent.signal(); + pThis->_transactionCS.leave(); + } + + pThis->onErrorPacketReceived(possiblePacket, packetStartRunningIndex); + + return; + } + + if (possiblePacket.isResponse()) { + if (pThis->_waitingForResponse) { + pThis->_transactionCS.enter(); + pThis->_receivedResponses.push(possiblePacket); + pThis->_newResponsesEvent.signal(); + pThis->_transactionCS.leave(); + } + + return; + } + + // This wasn't anything else. We assume it is an async packet. + pThis->onAsyncPacketReceived(possiblePacket, packetStartRunningIndex, timestamp); + } + + static void dataReceivedHandler(void * userData) + { + Impl * pi = static_cast(userData); + + size_t numOfBytesRead = 0; + + pi->port->read(pi->readBuffer, DefaultReadBufferSize, numOfBytesRead); + + if (numOfBytesRead == 0) return; + + TimeStamp t = TimeStamp::get(); + + if (pi->_rawDataReceivedHandler != NULL) + pi->_rawDataReceivedHandler( + pi->_rawDataReceivedUserData, reinterpret_cast(pi->readBuffer), numOfBytesRead, + pi->_dataRunningIndex); + +#if PYTHON + if (pi->_rawDataReceivedHandlerPython != NULL) { + vector pRawData(readBuffer, readBuffer + numOfBytesRead); + + python::AcquireGIL scopedLock; + + boost::python::call(pi->_rawDataReceivedHandlerPython, pRawData, pi->_dataRunningIndex); + } +#endif + + pi->_packetFinder.processReceivedData( + reinterpret_cast(pi->readBuffer), numOfBytesRead, pi->_filteringBootloaderResponses, + t); + + pi->_dataRunningIndex += numOfBytesRead; + } + + bool isConnected() { return port != NULL && port->isOpen(); } + + size_t finalizeCommandToSend(char * toSend, size_t length) + { +#if defined(_MSC_VER) +// Unable to use save version 'sprintf_s' since the length of 'toSend' is unknown here. +#pragma warning(push) +#pragma warning(disable : 4996) +#endif + + if (_sendErrorDetectionMode == ERRORDETECTIONMODE_CHECKSUM) { + length += sprintf(toSend + length, "%02X\r\n", Checksum8::compute(toSend + 1, length - 2)); + } else if (_sendErrorDetectionMode == ERRORDETECTIONMODE_CRC) { + length += sprintf(toSend + length, "%04X\r\n", Crc16::compute(toSend + 1, length - 2)); + } else { + length += sprintf(toSend + length, "XX\r\n"); + } + +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + + return length; + } + + Packet transactionWithWait( + char * toSend, size_t length, uint16_t responseTimeoutMs, uint16_t retransmitDelayMs) + { + // Make sure we don't have any existing responses. + _transactionCS.enter(); + +#if VN_SUPPORTS_SWAP + queue empty; + _receivedResponses.swap(empty); +#else + while (!_receivedResponses.empty()) _receivedResponses.pop(); +#endif + _waitingForResponse = true; + _transactionCS.leave(); + + // Send the command and continue sending if retransmits are enabled + // until we receive the response or timeout. + Stopwatch timeoutSw; + + port->write(toSend, length); + float curElapsedTime = timeoutSw.elapsedMs(); + + while (true) { + bool shouldRetransmit = false; + + // Compute how long we should wait for a response before taking + // more action. + float responseWaitTime = responseTimeoutMs - curElapsedTime; + if (responseWaitTime > retransmitDelayMs) { + responseWaitTime = retransmitDelayMs; + shouldRetransmit = true; + } + + // See if we have time left. + if (responseWaitTime < 0) { + _waitingForResponse = false; + throw timeout(); + } + + // Wait for any new responses that come in or until it is time to + // send a new retransmit. + xplat::Event::WaitResult waitResult = + _newResponsesEvent.waitUs(static_cast(responseWaitTime * 1000)); + + queue responsesToProcess; + + if (waitResult == xplat::Event::WAIT_TIMEDOUT) { + if (!shouldRetransmit) { + _waitingForResponse = false; + throw timeout(); + } + } + + if (waitResult == xplat::Event::WAIT_SIGNALED) { + // Get the current collection of responses. + _transactionCS.enter(); +#if VN_SUPPORTS_SWAP + _receivedResponses.swap(responsesToProcess); +#else + while (!_receivedResponses.empty()) { + responsesToProcess.push(_receivedResponses.front()); + _receivedResponses.pop(); + } +#endif + _transactionCS.leave(); + } + + // Process the collection of responses we have. + while (!responsesToProcess.empty()) { + Packet p = responsesToProcess.front(); + responsesToProcess.pop(); + + if (p.isError()) { + _waitingForResponse = false; + throw sensor_error(p.parseError()); + } + + // We must have a response packet. + _waitingForResponse = false; + Thread::sleepMs(1); + return p; + } + + // Retransmit. + port->write(toSend, length); + curElapsedTime = timeoutSw.elapsedMs(); + } + } + + void transactionNoFinalize( + char * toSend, size_t length, bool waitForReply, Packet * response, uint16_t responseTimeoutMs, + uint16_t retransmitDelayMs) + { + if (!isConnected()) throw invalid_operation(); + + if (waitForReply) { + *response = transactionWithWait(toSend, length, responseTimeoutMs, retransmitDelayMs); + + if (response->isError()) throw sensor_error(response->parseError()); + } else { + port->write(toSend, length); + } + } + + void transactionNoFinalize(char * toSend, size_t length, bool waitForReply, Packet * response) + { + transactionNoFinalize( + toSend, length, waitForReply, response, _responseTimeoutMs, _retransmitDelayMs); + } + + void transaction( + char * toSend, size_t length, bool waitForReply, Packet * response, uint16_t responseTimeoutMs, + uint16_t retransmitDelayMs) + { + length = finalizeCommandToSend(toSend, length); + + transactionNoFinalize( + toSend, length, waitForReply, response, responseTimeoutMs, retransmitDelayMs); + } + + // Performs a communication transaction with the sensor. If waitForReply is + // set to true, we will retransmit the message until we receive a response + // or until we time out, depending on current settings. + void transaction(char * toSend, size_t length, bool waitForReply, Packet * response) + { + transaction(toSend, length, waitForReply, response, _responseTimeoutMs, _retransmitDelayMs); + } + + BinaryOutputRegister readBinaryOutput(uint8_t binaryOutputNumber) + { + char toSend[17]; + Packet response; + uint16_t asyncMode, rateDivisor, outputGroup, commonField, timeField, imuField, gpsField, + attitudeField, insField, gps2Field; + +#if VN_HAVE_SECURE_CRT + int length = sprintf_s(toSend, sizeof(toSend), "$VNRRG,%u*", 74 + binaryOutputNumber); +#else + int length = sprintf(toSend, "$VNRRG,%u*", 74 + binaryOutputNumber); +#endif + + transaction(toSend, length, true, &response); + + response.parseBinaryOutput( + &asyncMode, &rateDivisor, &outputGroup, &commonField, &timeField, &imuField, &gpsField, + &attitudeField, &insField, &gps2Field); + + return BinaryOutputRegister( + static_cast(asyncMode), rateDivisor, static_cast(commonField), + static_cast(timeField), static_cast(imuField), + static_cast(gpsField), static_cast(attitudeField), + static_cast(insField), static_cast(gps2Field)); + } + + void writeBinaryOutput( + uint8_t binaryOutputNumber, BinaryOutputRegister & fields, bool waitForReply) + { + char toSend[256]; + Packet response; + + // First determine which groups are present. + uint16_t groups = 0; + if (fields.commonField) groups |= 0x0001; + if (fields.timeField) groups |= 0x0002; + if (fields.imuField) groups |= 0x0004; + if (fields.gpsField) groups |= 0x0008; + if (fields.attitudeField) groups |= 0x0010; + if (fields.insField) groups |= 0x0020; + if (fields.gps2Field) groups |= 0x0040; + +#if VN_HAVE_SECURE_CRT + int length = sprintf_s( + toSend, sizeof(toSend), "$VNWRG,%u,%u,%u,%X", 74 + binaryOutputNumber, fields.asyncMode, + fields.rateDivisor, groups); +#else + int length = sprintf( + toSend, "$VNWRG,%u,%u,%u,%X", 74 + binaryOutputNumber, fields.asyncMode, fields.rateDivisor, + groups); +#endif + + if (fields.commonField) +#if VN_HAVE_SECURE_CRT + length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.commonField); +#else + length += sprintf(toSend + length, ",%X", fields.commonField); +#endif + if (fields.timeField) +#if VN_HAVE_SECURE_CRT + length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.timeField); +#else + length += sprintf(toSend + length, ",%X", fields.timeField); +#endif + if (fields.imuField) +#if VN_HAVE_SECURE_CRT + length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.imuField); +#else + length += sprintf(toSend + length, ",%X", fields.imuField); +#endif + if (fields.gpsField) +#if VN_HAVE_SECURE_CRT + length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.gpsField); +#else + length += sprintf(toSend + length, ",%X", fields.gpsField); +#endif + if (fields.attitudeField) +#if VN_HAVE_SECURE_CRT + length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.attitudeField); +#else + length += sprintf(toSend + length, ",%X", fields.attitudeField); +#endif + if (fields.insField) +#if VN_HAVE_SECURE_CRT + length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.insField); +#else + length += sprintf(toSend + length, ",%X", fields.insField); +#endif + if (fields.gps2Field) +#if VN_HAVE_SECURE_CRT length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.gps2Field); - #else +#else length += sprintf(toSend + length, ",%X", fields.gps2Field); - #endif +#endif - #if VN_HAVE_SECURE_CRT - length += sprintf_s(toSend + length, sizeof(toSend) - length, "*"); - #else - length += sprintf(toSend + length, "*"); - #endif +#if VN_HAVE_SECURE_CRT + length += sprintf_s(toSend + length, sizeof(toSend) - length, "*"); +#else + length += sprintf(toSend + length, "*"); +#endif - transaction(toSend, length, waitForReply, &response); - } + transaction(toSend, length, waitForReply, &response); + } }; vector VnSensor::supportedBaudrates() { - uint32_t br[] = { - 9600, - 19200, - 38400, - 57600, - 115200, - 128000, - 230400, - 460800, - 921600 }; + uint32_t br[] = {9600, 19200, 38400, 57600, 115200, 128000, 230400, 460800, 921600}; - return vector(br, br + sizeof(br) / sizeof(uint32_t)); + return vector(br, br + sizeof(br) / sizeof(uint32_t)); } #if defined(_MSC_VER) && _MSC_VER <= 1600 - #pragma warning(push) - // Disable VS2010 warning for 'this' used in base member initializer list. - #pragma warning(disable:4355) +#pragma warning(push) +// Disable VS2010 warning for 'this' used in base member initializer list. +#pragma warning(disable : 4355) #endif -VnSensor::VnSensor() : - _pi(new Impl(this)) -{ - firmwareUpdateFile = NULL; - bootloaderFiltering = false; +VnSensor::VnSensor() : _pi(new Impl(this)) +{ + firmwareUpdateFile = NULL; + bootloaderFiltering = false; } #if defined(_MSC_VER) && _MSC_VER <= 1600 - #pragma warning(pop) +#pragma warning(pop) #endif VnSensor::~VnSensor() { - if (_pi != NULL) - { - if (_pi->SimplePortIsOurs && _pi->DidWeOpenSimplePort && isConnected()) - disconnect(); + if (_pi != NULL) { + if (_pi->SimplePortIsOurs && _pi->DidWeOpenSimplePort && isConnected()) disconnect(); - delete _pi; - _pi = NULL; - } + delete _pi; + _pi = NULL; + } } uint32_t VnSensor::baudrate() { - if (_pi->pSerialPort == NULL) - // We are not connected to a known serial port. - throw invalid_operation(); + if (_pi->pSerialPort == NULL) + // We are not connected to a known serial port. + throw invalid_operation(); - return _pi->pSerialPort->baudrate(); + return _pi->pSerialPort->baudrate(); } std::string VnSensor::port() { - if (_pi->pSerialPort == NULL) - // We are not connected to a known serial port. - throw invalid_operation(); + if (_pi->pSerialPort == NULL) + // We are not connected to a known serial port. + throw invalid_operation(); - return _pi->pSerialPort->port(); + return _pi->pSerialPort->port(); } -ErrorDetectionMode VnSensor::sendErrorDetectionMode() -{ - return _pi->_sendErrorDetectionMode; -} +ErrorDetectionMode VnSensor::sendErrorDetectionMode() { return _pi->_sendErrorDetectionMode; } void VnSensor::setSendErrorDetectionMode(ErrorDetectionMode mode) { - _pi->_sendErrorDetectionMode = mode; + _pi->_sendErrorDetectionMode = mode; } -bool VnSensor::isConnected() -{ - return _pi->isConnected(); -} +bool VnSensor::isConnected() { return _pi->isConnected(); } -uint16_t VnSensor::responseTimeoutMs() -{ - return _pi->_responseTimeoutMs; -} +uint16_t VnSensor::responseTimeoutMs() { return _pi->_responseTimeoutMs; } -void VnSensor::setResponseTimeoutMs(uint16_t timeout) -{ - _pi->_responseTimeoutMs = timeout; -} +void VnSensor::setResponseTimeoutMs(uint16_t timeout) { _pi->_responseTimeoutMs = timeout; } -uint16_t VnSensor::retransmitDelayMs() -{ - return _pi->_retransmitDelayMs; -} +uint16_t VnSensor::retransmitDelayMs() { return _pi->_retransmitDelayMs; } -void VnSensor::setRetransmitDelayMs(uint16_t delay) -{ - _pi->_retransmitDelayMs = delay; -} +void VnSensor::setRetransmitDelayMs(uint16_t delay) { _pi->_retransmitDelayMs = delay; } bool VnSensor::verifySensorConnectivity() { - try - { - readModelNumber(); + try { + readModelNumber(); - return true; - } - catch (...) { } + return true; + } catch (...) { + } - return false; + return false; } -void VnSensor::connect(const string &portName, uint32_t baudrate) +void VnSensor::connect(const string & portName, uint32_t baudrate) { - _pi->pSerialPort = new SerialPort(portName, baudrate); + _pi->pSerialPort = new SerialPort(portName, baudrate); - connect(dynamic_cast(_pi->pSerialPort)); + connect(dynamic_cast(_pi->pSerialPort)); } -void VnSensor::connect(IPort* simplePort) +void VnSensor::connect(IPort * simplePort) { - _pi->port = simplePort; - _pi->SimplePortIsOurs = false; + _pi->port = simplePort; + _pi->SimplePortIsOurs = false; - _pi->port->registerDataReceivedHandler(_pi, Impl::dataReceivedHandler); + _pi->port->registerDataReceivedHandler(_pi, Impl::dataReceivedHandler); - if (!_pi->port->isOpen()) - { - _pi->port->open(); - _pi->DidWeOpenSimplePort = true; - } + if (!_pi->port->isOpen()) { + _pi->port->open(); + _pi->DidWeOpenSimplePort = true; + } } void VnSensor::disconnect() { - if (_pi->port == NULL || !_pi->port->isOpen()) - throw invalid_operation(); + if (_pi->port == NULL || !_pi->port->isOpen()) throw invalid_operation(); - _pi->port->unregisterDataReceivedHandler(); + _pi->port->unregisterDataReceivedHandler(); - if (_pi->DidWeOpenSimplePort) - { - _pi->port->close(); - } + if (_pi->DidWeOpenSimplePort) { + _pi->port->close(); + } - _pi->DidWeOpenSimplePort = false; + _pi->DidWeOpenSimplePort = false; - if (_pi->SimplePortIsOurs) - { - delete _pi->port; - } - _pi->port = NULL; + if (_pi->SimplePortIsOurs) { + delete _pi->port; + } + _pi->port = NULL; - if (_pi->pSerialPort != NULL) - { - // Assuming we created this serial port. - delete _pi->pSerialPort; - _pi->pSerialPort = NULL; - } + if (_pi->pSerialPort != NULL) { + // Assuming we created this serial port. + delete _pi->pSerialPort; + _pi->pSerialPort = NULL; + } } string VnSensor::transaction(string toSend) { - char buffer[COMMAND_MAX_LENGTH]; - size_t finalLength = toSend.length(); - Packet response; + char buffer[COMMAND_MAX_LENGTH]; + size_t finalLength = toSend.length(); + Packet response; - // Copy over what was provided. - copy(toSend.begin(), toSend.end(), buffer); + // Copy over what was provided. + copy(toSend.begin(), toSend.end(), buffer); - // First see if an '*' is present. - if (toSend.find('*') == string::npos) - { - buffer[toSend.length()] = '*'; - finalLength = _pi->finalizeCommandToSend(buffer, toSend.length() + 1); - } - else if (toSend[toSend.length() - 2] != '\r' && toSend[toSend.length() - 1] != '\n') - { - buffer[toSend.length()] = '\r'; - buffer[toSend.length() + 1] = '\n'; - finalLength += 2; - } + // First see if an '*' is present. + if (toSend.find('*') == string::npos) { + buffer[toSend.length()] = '*'; + finalLength = _pi->finalizeCommandToSend(buffer, toSend.length() + 1); + } else if (toSend[toSend.length() - 2] != '\r' && toSend[toSend.length() - 1] != '\n') { + buffer[toSend.length()] = '\r'; + buffer[toSend.length() + 1] = '\n'; + finalLength += 2; + } - _pi->transactionNoFinalize(buffer, finalLength, true, &response); + _pi->transactionNoFinalize(buffer, finalLength, true, &response); - return response.datastr(); + return response.datastr(); } string VnSensor::send(string toSend, bool waitForReply, ErrorDetectionMode errorDetectionMode) { - Packet p; - char *buffer = new char[toSend.size() + 8]; // Extra room for possible additions. - size_t curToSendLength = toSend.size(); - - // See if a '$' needs to be prepended. - if (toSend[0] == '$') - { - #if VN_HAVE_SECURE_SCL - toSend._Copy_s(buffer, toSend.size() + 8, toSend.size()); - #else - toSend.copy(buffer, toSend.size()); - #endif - - } - else - { - buffer[0] = '$'; - #if VN_HAVE_SECURE_SCL - toSend._Copy_s(buffer + 1, toSend.size() + 8 - 1, toSend.size()); - #else - toSend.copy(buffer + 1, toSend.size()); - #endif - curToSendLength++; - } - - // Locate any '*' in the command. - size_t astrickLocation = string::npos; - for (size_t i = 0; i < curToSendLength; i++) - { - if (buffer[i] == '*') - { - astrickLocation = i; - break; - } - } - - // Do we need to add a '*'? - if (astrickLocation == string::npos) - { - buffer[curToSendLength] = '*'; - astrickLocation = curToSendLength++; - } - - // Do we need to add a checksum/CRC? - if (astrickLocation == curToSendLength - 1) - { - if (errorDetectionMode == ERRORDETECTIONMODE_CHECKSUM) - { - #if VN_HAVE_SECURE_CRT - curToSendLength += sprintf_s(buffer + curToSendLength, toSend.size() + 8 - curToSendLength, "%02X\r\n", Checksum8::compute(buffer + 1, curToSendLength - 2)); - #else - curToSendLength += sprintf(buffer + curToSendLength, "%02X\r\n", Checksum8::compute(buffer + 1, curToSendLength - 2)); - #endif - } - else if (errorDetectionMode == ERRORDETECTIONMODE_CRC) - { - #if VN_HAVE_SECURE_CRT - curToSendLength += sprintf_s(buffer + curToSendLength, toSend.size() + 8 - curToSendLength, "%04X\r\n", Crc16::compute(buffer + 1, curToSendLength - 2)); - #else - curToSendLength += sprintf(buffer + curToSendLength, "%04X\r\n", Crc16::compute(buffer + 1, curToSendLength - 2)); - #endif - } - else - { - #if VN_HAVE_SECURE_CRT - curToSendLength += sprintf_s(buffer + curToSendLength, toSend.size() + 8 - curToSendLength, "*XX\r\n"); - #else - curToSendLength += sprintf(buffer + curToSendLength, "*XX\r\n"); - #endif - } - } - // Do we need to add "\r\n"? - else if (buffer[curToSendLength - 1] != '\n') - { - buffer[curToSendLength++] = '\r'; - buffer[curToSendLength++] = '\n'; - } - - _pi->transactionNoFinalize(buffer, curToSendLength, waitForReply, &p, _pi->_responseTimeoutMs, _pi->_retransmitDelayMs); - - delete [] buffer; - - return p.datastr(); -} - -void VnSensor::registerRawDataReceivedHandler(void* userData, RawDataReceivedHandler handler) -{ - if (_pi->_rawDataReceivedHandler != NULL) - throw invalid_operation(); - - _pi->_rawDataReceivedHandler = handler; - _pi->_rawDataReceivedUserData = userData; + Packet p; + char * buffer = new char[toSend.size() + 8]; // Extra room for possible additions. + size_t curToSendLength = toSend.size(); + + // See if a '$' needs to be prepended. + if (toSend[0] == '$') { +#if VN_HAVE_SECURE_SCL + toSend._Copy_s(buffer, toSend.size() + 8, toSend.size()); +#else + toSend.copy(buffer, toSend.size()); +#endif + + } else { + buffer[0] = '$'; +#if VN_HAVE_SECURE_SCL + toSend._Copy_s(buffer + 1, toSend.size() + 8 - 1, toSend.size()); +#else + toSend.copy(buffer + 1, toSend.size()); +#endif + curToSendLength++; + } + + // Locate any '*' in the command. + size_t astrickLocation = string::npos; + for (size_t i = 0; i < curToSendLength; i++) { + if (buffer[i] == '*') { + astrickLocation = i; + break; + } + } + + // Do we need to add a '*'? + if (astrickLocation == string::npos) { + buffer[curToSendLength] = '*'; + astrickLocation = curToSendLength++; + } + + // Do we need to add a checksum/CRC? + if (astrickLocation == curToSendLength - 1) { + if (errorDetectionMode == ERRORDETECTIONMODE_CHECKSUM) { +#if VN_HAVE_SECURE_CRT + curToSendLength += sprintf_s( + buffer + curToSendLength, toSend.size() + 8 - curToSendLength, "%02X\r\n", + Checksum8::compute(buffer + 1, curToSendLength - 2)); +#else + curToSendLength += sprintf( + buffer + curToSendLength, "%02X\r\n", Checksum8::compute(buffer + 1, curToSendLength - 2)); +#endif + } else if (errorDetectionMode == ERRORDETECTIONMODE_CRC) { +#if VN_HAVE_SECURE_CRT + curToSendLength += sprintf_s( + buffer + curToSendLength, toSend.size() + 8 - curToSendLength, "%04X\r\n", + Crc16::compute(buffer + 1, curToSendLength - 2)); +#else + curToSendLength += sprintf( + buffer + curToSendLength, "%04X\r\n", Crc16::compute(buffer + 1, curToSendLength - 2)); +#endif + } else { +#if VN_HAVE_SECURE_CRT + curToSendLength += + sprintf_s(buffer + curToSendLength, toSend.size() + 8 - curToSendLength, "*XX\r\n"); +#else + curToSendLength += sprintf(buffer + curToSendLength, "*XX\r\n"); +#endif + } + } + // Do we need to add "\r\n"? + else if (buffer[curToSendLength - 1] != '\n') { + buffer[curToSendLength++] = '\r'; + buffer[curToSendLength++] = '\n'; + } + + _pi->transactionNoFinalize( + buffer, curToSendLength, waitForReply, &p, _pi->_responseTimeoutMs, _pi->_retransmitDelayMs); + + delete[] buffer; + + return p.datastr(); +} + +void VnSensor::registerRawDataReceivedHandler(void * userData, RawDataReceivedHandler handler) +{ + if (_pi->_rawDataReceivedHandler != NULL) throw invalid_operation(); + + _pi->_rawDataReceivedHandler = handler; + _pi->_rawDataReceivedUserData = userData; } #if PL150 @@ -831,12 +744,12 @@ void VnSensor::registerRawDataReceivedHandler(void* userData, RawDataReceivedHan #if PYTHON -PyObject* VnSensor::registerRawDataReceivedHandler(PyObject* callable) +PyObject * VnSensor::registerRawDataReceivedHandler(PyObject * callable) { - _pi->_rawDataReceivedHandlerPython = callable; - callable->ob_refcnt++; + _pi->_rawDataReceivedHandlerPython = callable; + callable->ob_refcnt++; - return Py_None; + return Py_None; } #endif @@ -844,38 +757,36 @@ PyObject* VnSensor::registerRawDataReceivedHandler(PyObject* callable) void VnSensor::unregisterRawDataReceivedHandler() { - if (_pi->_rawDataReceivedHandler == NULL) - throw invalid_operation(); + if (_pi->_rawDataReceivedHandler == NULL) throw invalid_operation(); - _pi->_rawDataReceivedHandler = NULL; - _pi->_rawDataReceivedUserData = NULL; + _pi->_rawDataReceivedHandler = NULL; + _pi->_rawDataReceivedUserData = NULL; } -void VnSensor::registerPossiblePacketFoundHandler(void* userData, PossiblePacketFoundHandler handler) +void VnSensor::registerPossiblePacketFoundHandler( + void * userData, PossiblePacketFoundHandler handler) { - if (_pi->_possiblePacketFoundHandler != NULL) - throw invalid_operation(); + if (_pi->_possiblePacketFoundHandler != NULL) throw invalid_operation(); - _pi->_possiblePacketFoundHandler = handler; - _pi->_possiblePacketFoundUserData = userData; + _pi->_possiblePacketFoundHandler = handler; + _pi->_possiblePacketFoundUserData = userData; } void VnSensor::unregisterPossiblePacketFoundHandler() { - if (_pi->_possiblePacketFoundHandler == NULL) - throw invalid_operation(); + if (_pi->_possiblePacketFoundHandler == NULL) throw invalid_operation(); - _pi->_possiblePacketFoundHandler = NULL; - _pi->_possiblePacketFoundUserData = NULL; + _pi->_possiblePacketFoundHandler = NULL; + _pi->_possiblePacketFoundUserData = NULL; } -void VnSensor::registerAsyncPacketReceivedHandler(void* userData, AsyncPacketReceivedHandler handler) +void VnSensor::registerAsyncPacketReceivedHandler( + void * userData, AsyncPacketReceivedHandler handler) { - if (_pi->_asyncPacketReceivedHandler != NULL) - throw invalid_operation(); + if (_pi->_asyncPacketReceivedHandler != NULL) throw invalid_operation(); - _pi->_asyncPacketReceivedHandler = handler; - _pi->_asyncPacketReceivedUserData = userData; + _pi->_asyncPacketReceivedHandler = handler; + _pi->_asyncPacketReceivedUserData = userData; } #if PL150 @@ -883,12 +794,12 @@ void VnSensor::registerAsyncPacketReceivedHandler(void* userData, AsyncPacketRec #else #if PYTHON -PyObject* VnSensor::registerAsyncPacketReceivedHandler(PyObject* callable) +PyObject * VnSensor::registerAsyncPacketReceivedHandler(PyObject * callable) { - _pi->_asyncPacketReceivedHandlerPython = callable; - callable->ob_refcnt++; + _pi->_asyncPacketReceivedHandlerPython = callable; + callable->ob_refcnt++; - return Py_None; + return Py_None; } #endif @@ -896,2854 +807,2517 @@ PyObject* VnSensor::registerAsyncPacketReceivedHandler(PyObject* callable) void VnSensor::unregisterAsyncPacketReceivedHandler() { - if (_pi->_asyncPacketReceivedHandler == NULL) - throw invalid_operation(); + if (_pi->_asyncPacketReceivedHandler == NULL) throw invalid_operation(); - _pi->_asyncPacketReceivedHandler = NULL; - _pi->_asyncPacketReceivedUserData = NULL; + _pi->_asyncPacketReceivedHandler = NULL; + _pi->_asyncPacketReceivedUserData = NULL; } -void VnSensor::registerErrorPacketReceivedHandler(void* userData, ErrorPacketReceivedHandler handler) +void VnSensor::registerErrorPacketReceivedHandler( + void * userData, ErrorPacketReceivedHandler handler) { - if (_pi->_errorPacketReceivedHandler != NULL) - throw invalid_operation(); + if (_pi->_errorPacketReceivedHandler != NULL) throw invalid_operation(); - _pi->_errorPacketReceivedHandler = handler; - _pi->_errorPacketReceivedUserData = userData; + _pi->_errorPacketReceivedHandler = handler; + _pi->_errorPacketReceivedUserData = userData; } void VnSensor::unregisterErrorPacketReceivedHandler() { - if (_pi->_errorPacketReceivedHandler == NULL) - throw invalid_operation(); + if (_pi->_errorPacketReceivedHandler == NULL) throw invalid_operation(); - _pi->_errorPacketReceivedHandler = NULL; - _pi->_errorPacketReceivedUserData = NULL; + _pi->_errorPacketReceivedHandler = NULL; + _pi->_errorPacketReceivedUserData = NULL; } void VnSensor::writeSettings(bool waitForReply) { - char toSend[37]; + char toSend[37]; - size_t length = Packet::genWriteSettings(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genWriteSettings(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; + Packet response; - // Write settings sometimes takes a while to do and receive a response - // from the sensor. - _pi->transactionNoFinalize(toSend, length, waitForReply, &response, 2500, 1000); + // Write settings sometimes takes a while to do and receive a response + // from the sensor. + _pi->transactionNoFinalize(toSend, length, waitForReply, &response, 2500, 1000); } void VnSensor::tare(bool waitForReply) { - char toSend[14]; + char toSend[14]; - size_t length = Packet::genTare(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genTare(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::setGyroBias(bool waitForReply) { - char toSend[14]; + char toSend[14]; - size_t length = Packet::genSetGyroBias(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genSetGyroBias(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::magneticDisturbancePresent(bool disturbancePresent, bool waitForReply) { - char toSend[16]; + char toSend[16]; - size_t length = Packet::genKnownMagneticDisturbance(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), disturbancePresent); + size_t length = Packet::genKnownMagneticDisturbance( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), disturbancePresent); - Packet response; + Packet response; - // Write settings sometimes takes a while to do and receive a response - // from the sensor. - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + // Write settings sometimes takes a while to do and receive a response + // from the sensor. + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::accelerationDisturbancePresent(bool disturbancePresent, bool waitForReply) { - char toSend[16]; + char toSend[16]; - size_t length = Packet::genKnownAccelerationDisturbance(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), disturbancePresent); + size_t length = Packet::genKnownAccelerationDisturbance( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), disturbancePresent); - Packet response; + Packet response; - // Write settings sometimes takes a while to do and receive a response - // from the sensor. - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + // Write settings sometimes takes a while to do and receive a response + // from the sensor. + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::restoreFactorySettings(bool waitForReply) { - char toSend[37]; + char toSend[37]; - size_t length = Packet::genRestoreFactorySettings(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genRestoreFactorySettings(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; + Packet response; - // Restore factory settings sometimes takes a while to do and receive a - // response from the sensor. - _pi->transactionNoFinalize(toSend, length, waitForReply, &response, 2500, 1000); + // Restore factory settings sometimes takes a while to do and receive a + // response from the sensor. + _pi->transactionNoFinalize(toSend, length, waitForReply, &response, 2500, 1000); } void VnSensor::reset(bool waitForReply) { - char toSend[37]; + char toSend[37]; - size_t length = Packet::genReset(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReset(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; + Packet response; - // Reset sometimes takes a while to do and receive a response from the - // sensor. - _pi->transactionNoFinalize(toSend, length, waitForReply, &response, 2500, 1000); + // Reset sometimes takes a while to do and receive a response from the + // sensor. + _pi->transactionNoFinalize(toSend, length, waitForReply, &response, 2500, 1000); } void VnSensor::firmwareUpdateMode(bool waitForReply) { - char toSend[37]; + char toSend[37]; - size_t length = Packet::genFirmwareUpdate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genFirmwareUpdate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; + Packet response; - // Reset sometimes takes a while to do and receive a response from the sensor. - _pi->transactionNoFinalize(toSend, length, waitForReply, &response, 3000, 3000); + // Reset sometimes takes a while to do and receive a response from the sensor. + _pi->transactionNoFinalize(toSend, length, waitForReply, &response, 3000, 3000); } - void VnSensor::changeBaudRate(uint32_t baudrate) { - writeSerialBaudRate(baudrate, true); + writeSerialBaudRate(baudrate, true); - _pi->pSerialPort->changeBaudrate(baudrate); + _pi->pSerialPort->changeBaudrate(baudrate); } VnSensor::Family VnSensor::determineDeviceFamily() { - string mn = readModelNumber(); + string mn = readModelNumber(); - return determineDeviceFamily(mn); + return determineDeviceFamily(mn); } VnSensor::Family VnSensor::determineDeviceFamily(std::string modelNumber) { - if (modelNumber.find("VN-100") == 0) - return VnSensor_Family_Vn100; - - if (modelNumber.find("VN-200") == 0) - return VnSensor_Family_Vn200; + if (modelNumber.find("VN-100") == 0) return VnSensor_Family_Vn100; - if (modelNumber.find("VN-300") == 0) - return VnSensor_Family_Vn300; + if (modelNumber.find("VN-200") == 0) return VnSensor_Family_Vn200; - return VnSensor_Family_Unknown; -} + if (modelNumber.find("VN-300") == 0) return VnSensor_Family_Vn300; -BinaryOutputRegister VnSensor::readBinaryOutput1() -{ - return _pi->readBinaryOutput(1); + return VnSensor_Family_Unknown; } -BinaryOutputRegister VnSensor::readBinaryOutput2() -{ - return _pi->readBinaryOutput(2); -} +BinaryOutputRegister VnSensor::readBinaryOutput1() { return _pi->readBinaryOutput(1); } -BinaryOutputRegister VnSensor::readBinaryOutput3() -{ - return _pi->readBinaryOutput(3); -} +BinaryOutputRegister VnSensor::readBinaryOutput2() { return _pi->readBinaryOutput(2); } +BinaryOutputRegister VnSensor::readBinaryOutput3() { return _pi->readBinaryOutput(3); } -void VnSensor::writeBinaryOutput1(BinaryOutputRegister &fields, bool waitForReply) +void VnSensor::writeBinaryOutput1(BinaryOutputRegister & fields, bool waitForReply) { - _pi->writeBinaryOutput(1, fields, waitForReply); + _pi->writeBinaryOutput(1, fields, waitForReply); } -void VnSensor::writeBinaryOutput2(BinaryOutputRegister &fields, bool waitForReply) +void VnSensor::writeBinaryOutput2(BinaryOutputRegister & fields, bool waitForReply) { - _pi->writeBinaryOutput(2, fields, waitForReply); + _pi->writeBinaryOutput(2, fields, waitForReply); } -void VnSensor::writeBinaryOutput3(BinaryOutputRegister &fields, bool waitForReply) +void VnSensor::writeBinaryOutput3(BinaryOutputRegister & fields, bool waitForReply) { - _pi->writeBinaryOutput(3, fields, waitForReply); + _pi->writeBinaryOutput(3, fields, waitForReply); } - uint32_t VnSensor::readSerialBaudRate(uint8_t port) { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadSerialBaudRate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), port); + size_t length = + Packet::genReadSerialBaudRate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), port); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - uint32_t reg; - response.parseSerialBaudRate(®); + uint32_t reg; + response.parseSerialBaudRate(®); - return reg; + return reg; } -void VnSensor::writeSerialBaudRate(const uint32_t &baudrate, uint8_t port, bool waitForReply) +void VnSensor::writeSerialBaudRate(const uint32_t & baudrate, uint8_t port, bool waitForReply) { - char toSend[25]; + char toSend[25]; - size_t length = Packet::genWriteSerialBaudRate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), baudrate, port); + size_t length = Packet::genWriteSerialBaudRate( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), baudrate, port); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } AsciiAsync VnSensor::readAsyncDataOutputType(uint8_t port) { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadAsyncDataOutputType(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), port); + size_t length = + Packet::genReadAsyncDataOutputType(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), port); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - uint32_t reg; - response.parseAsyncDataOutputType(®); + uint32_t reg; + response.parseAsyncDataOutputType(®); - return static_cast(reg); + return static_cast(reg); } void VnSensor::writeAsyncDataOutputType(AsciiAsync ador, uint8_t port, bool waitForReply) { - char toSend[21]; + char toSend[21]; - size_t length = Packet::genWriteAsyncDataOutputType(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), ador, port); + size_t length = Packet::genWriteAsyncDataOutputType( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), ador, port); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } uint32_t VnSensor::readAsyncDataOutputFrequency(uint8_t port) { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadAsyncDataOutputFrequency(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), port); + size_t length = Packet::genReadAsyncDataOutputFrequency( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), port); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - uint32_t reg; - response.parseAsyncDataOutputFrequency(®); + uint32_t reg; + response.parseAsyncDataOutputFrequency(®); - return reg; + return reg; } -void VnSensor::writeAsyncDataOutputFrequency(const uint32_t &adof, uint8_t port, bool waitForReply) +void VnSensor::writeAsyncDataOutputFrequency(const uint32_t & adof, uint8_t port, bool waitForReply) { - char toSend[26]; + char toSend[26]; - size_t length = Packet::genWriteAsyncDataOutputFrequency(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), adof, port); + size_t length = Packet::genWriteAsyncDataOutputFrequency( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), adof, port); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } InsBasicConfigurationRegisterVn200 VnSensor::readInsBasicConfigurationVn200() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadInsBasicConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadInsBasicConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - InsBasicConfigurationRegisterVn200 reg; - uint8_t scenario; - uint8_t ahrsAiding; - response.parseInsBasicConfiguration( - &scenario, - &ahrsAiding); + InsBasicConfigurationRegisterVn200 reg; + uint8_t scenario; + uint8_t ahrsAiding; + response.parseInsBasicConfiguration(&scenario, &ahrsAiding); - reg.scenario = static_cast(scenario); - reg.ahrsAiding = ahrsAiding != 0; + reg.scenario = static_cast(scenario); + reg.ahrsAiding = ahrsAiding != 0; - return reg; + return reg; } -void VnSensor::writeInsBasicConfigurationVn200(InsBasicConfigurationRegisterVn200 &fields, bool waitForReply) +void VnSensor::writeInsBasicConfigurationVn200( + InsBasicConfigurationRegisterVn200 & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteInsBasicConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.scenario, fields.ahrsAiding, 0); + size_t length = Packet::genWriteInsBasicConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.scenario, fields.ahrsAiding, 0); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeInsBasicConfigurationVn200( - Scenario scenario, - const uint8_t &ahrsAiding, - bool waitForReply) + Scenario scenario, const uint8_t & ahrsAiding, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteInsBasicConfiguration( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - scenario, - ahrsAiding, - 0); + size_t length = Packet::genWriteInsBasicConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), scenario, ahrsAiding, 0); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } InsBasicConfigurationRegisterVn300 VnSensor::readInsBasicConfigurationVn300() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadInsBasicConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadInsBasicConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - InsBasicConfigurationRegisterVn300 reg; - uint8_t scenario; - uint8_t ahrsAiding; - uint8_t estBaseline; - response.parseInsBasicConfiguration( - &scenario, - &ahrsAiding, - &estBaseline); + InsBasicConfigurationRegisterVn300 reg; + uint8_t scenario; + uint8_t ahrsAiding; + uint8_t estBaseline; + response.parseInsBasicConfiguration(&scenario, &ahrsAiding, &estBaseline); - reg.scenario = static_cast(scenario); - reg.ahrsAiding = ahrsAiding != 0; - reg.estBaseline = estBaseline != 0; + reg.scenario = static_cast(scenario); + reg.ahrsAiding = ahrsAiding != 0; + reg.estBaseline = estBaseline != 0; - return reg; + return reg; } -void VnSensor::writeInsBasicConfigurationVn300(InsBasicConfigurationRegisterVn300 &fields, bool waitForReply) +void VnSensor::writeInsBasicConfigurationVn300( + InsBasicConfigurationRegisterVn300 & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteInsBasicConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.scenario, fields.ahrsAiding, fields.estBaseline); + size_t length = Packet::genWriteInsBasicConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.scenario, fields.ahrsAiding, + fields.estBaseline); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeInsBasicConfigurationVn300( - Scenario scenario, - const uint8_t &ahrsAiding, - const uint8_t &estBaseline, - bool waitForReply) + Scenario scenario, const uint8_t & ahrsAiding, const uint8_t & estBaseline, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteInsBasicConfiguration( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - scenario, - ahrsAiding, - estBaseline); + size_t length = Packet::genWriteInsBasicConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), scenario, ahrsAiding, estBaseline); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } string VnSensor::readUserTag() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadUserTag(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadUserTag(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - char tagBuffer[50]; - response.parseUserTag(tagBuffer); + char tagBuffer[50]; + response.parseUserTag(tagBuffer); - return tagBuffer; + return tagBuffer; } -void VnSensor::writeUserTag(const string &tag, bool waitForReply) +void VnSensor::writeUserTag(const string & tag, bool waitForReply) { - char toSend[37]; + char toSend[37]; - size_t length = Packet::genWriteUserTag(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), tag.c_str()); + size_t length = + Packet::genWriteUserTag(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), tag.c_str()); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } string VnSensor::readModelNumber() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadModelNumber(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadModelNumber(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - char productNameBuffer[50]; - response.parseModelNumber(productNameBuffer); + char productNameBuffer[50]; + response.parseModelNumber(productNameBuffer); - return productNameBuffer; + return productNameBuffer; } uint32_t VnSensor::readHardwareRevision() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadHardwareRevision(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadHardwareRevision(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - uint32_t reg; - response.parseHardwareRevision(®); + uint32_t reg; + response.parseHardwareRevision(®); - return reg; + return reg; } uint32_t VnSensor::readSerialNumber() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadSerialNumber(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadSerialNumber(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - uint32_t reg; - response.parseSerialNumber(®); + uint32_t reg; + response.parseSerialNumber(®); - return reg; + return reg; } string VnSensor::readFirmwareVersion() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadFirmwareVersion(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadFirmwareVersion(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - char firmwareVersionBuffer[50]; - response.parseFirmwareVersion(firmwareVersionBuffer); + char firmwareVersionBuffer[50]; + response.parseFirmwareVersion(firmwareVersionBuffer); - return firmwareVersionBuffer; + return firmwareVersionBuffer; } uint32_t VnSensor::readSerialBaudRate() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadSerialBaudRate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadSerialBaudRate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - uint32_t reg; - response.parseSerialBaudRate(®); + uint32_t reg; + response.parseSerialBaudRate(®); - return reg; + return reg; } -void VnSensor::writeSerialBaudRate(const uint32_t &baudrate, bool waitForReply) +void VnSensor::writeSerialBaudRate(const uint32_t & baudrate, bool waitForReply) { - char toSend[25]; + char toSend[25]; - size_t length = Packet::genWriteSerialBaudRate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), baudrate); + size_t length = + Packet::genWriteSerialBaudRate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), baudrate); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } AsciiAsync VnSensor::readAsyncDataOutputType() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadAsyncDataOutputType(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadAsyncDataOutputType(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - uint32_t reg; - response.parseAsyncDataOutputType(®); + uint32_t reg; + response.parseAsyncDataOutputType(®); - return static_cast(reg); + return static_cast(reg); } void VnSensor::writeAsyncDataOutputType(AsciiAsync ador, bool waitForReply) { - char toSend[19]; + char toSend[19]; - size_t length = Packet::genWriteAsyncDataOutputType(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), ador); + size_t length = + Packet::genWriteAsyncDataOutputType(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), ador); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } uint32_t VnSensor::readAsyncDataOutputFrequency() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadAsyncDataOutputFrequency(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadAsyncDataOutputFrequency(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - uint32_t reg; - response.parseAsyncDataOutputFrequency(®); + uint32_t reg; + response.parseAsyncDataOutputFrequency(®); - return reg; + return reg; } -void VnSensor::writeAsyncDataOutputFrequency(const uint32_t &adof, bool waitForReply) +void VnSensor::writeAsyncDataOutputFrequency(const uint32_t & adof, bool waitForReply) { - char toSend[26]; + char toSend[26]; - size_t length = Packet::genWriteAsyncDataOutputFrequency(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), adof); + size_t length = Packet::genWriteAsyncDataOutputFrequency( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), adof); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } vec3f VnSensor::readYawPitchRoll() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadYawPitchRoll(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadYawPitchRoll(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - vec3f reg; - response.parseYawPitchRoll(®); + vec3f reg; + response.parseYawPitchRoll(®); - return reg; + return reg; } vec4f VnSensor::readAttitudeQuaternion() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadAttitudeQuaternion(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadAttitudeQuaternion(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - vec4f reg; - response.parseAttitudeQuaternion(®); + vec4f reg; + response.parseAttitudeQuaternion(®); - return reg; + return reg; } -QuaternionMagneticAccelerationAndAngularRatesRegister VnSensor::readQuaternionMagneticAccelerationAndAngularRates() +QuaternionMagneticAccelerationAndAngularRatesRegister +VnSensor::readQuaternionMagneticAccelerationAndAngularRates() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadQuaternionMagneticAccelerationAndAngularRates(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadQuaternionMagneticAccelerationAndAngularRates( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - QuaternionMagneticAccelerationAndAngularRatesRegister reg; - response.parseQuaternionMagneticAccelerationAndAngularRates( - ®.quat, - ®.mag, - ®.accel, - ®.gyro); + QuaternionMagneticAccelerationAndAngularRatesRegister reg; + response.parseQuaternionMagneticAccelerationAndAngularRates( + ®.quat, ®.mag, ®.accel, ®.gyro); - return reg; + return reg; } vec3f VnSensor::readMagneticMeasurements() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadMagneticMeasurements(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadMagneticMeasurements(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - vec3f reg; - response.parseMagneticMeasurements(®); + vec3f reg; + response.parseMagneticMeasurements(®); - return reg; + return reg; } vec3f VnSensor::readAccelerationMeasurements() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadAccelerationMeasurements(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadAccelerationMeasurements(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - vec3f reg; - response.parseAccelerationMeasurements(®); + vec3f reg; + response.parseAccelerationMeasurements(®); - return reg; + return reg; } vec3f VnSensor::readAngularRateMeasurements() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadAngularRateMeasurements(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadAngularRateMeasurements(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - vec3f reg; - response.parseAngularRateMeasurements(®); + vec3f reg; + response.parseAngularRateMeasurements(®); - return reg; + return reg; } MagneticAccelerationAndAngularRatesRegister VnSensor::readMagneticAccelerationAndAngularRates() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadMagneticAccelerationAndAngularRates(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadMagneticAccelerationAndAngularRates( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - MagneticAccelerationAndAngularRatesRegister reg; - response.parseMagneticAccelerationAndAngularRates( - ®.mag, - ®.accel, - ®.gyro); + MagneticAccelerationAndAngularRatesRegister reg; + response.parseMagneticAccelerationAndAngularRates(®.mag, ®.accel, ®.gyro); - return reg; + return reg; } MagneticAndGravityReferenceVectorsRegister VnSensor::readMagneticAndGravityReferenceVectors() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadMagneticAndGravityReferenceVectors(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadMagneticAndGravityReferenceVectors( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - MagneticAndGravityReferenceVectorsRegister reg; - response.parseMagneticAndGravityReferenceVectors( - ®.magRef, - ®.accRef); + MagneticAndGravityReferenceVectorsRegister reg; + response.parseMagneticAndGravityReferenceVectors(®.magRef, ®.accRef); - return reg; + return reg; } -void VnSensor::writeMagneticAndGravityReferenceVectors(MagneticAndGravityReferenceVectorsRegister &fields, bool waitForReply) +void VnSensor::writeMagneticAndGravityReferenceVectors( + MagneticAndGravityReferenceVectorsRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteMagneticAndGravityReferenceVectors(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.magRef, fields.accRef); + size_t length = Packet::genWriteMagneticAndGravityReferenceVectors( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.magRef, fields.accRef); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeMagneticAndGravityReferenceVectors( - const vec3f &magRef, - const vec3f &accRef, - bool waitForReply) + const vec3f & magRef, const vec3f & accRef, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteMagneticAndGravityReferenceVectors( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - magRef, - accRef); + size_t length = Packet::genWriteMagneticAndGravityReferenceVectors( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), magRef, accRef); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } FilterMeasurementsVarianceParametersRegister VnSensor::readFilterMeasurementsVarianceParameters() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadFilterMeasurementsVarianceParameters(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadFilterMeasurementsVarianceParameters( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - FilterMeasurementsVarianceParametersRegister reg; - response.parseFilterMeasurementsVarianceParameters( - ®.angularWalkVariance, - ®.angularRateVariance, - ®.magneticVariance, - ®.accelerationVariance); + FilterMeasurementsVarianceParametersRegister reg; + response.parseFilterMeasurementsVarianceParameters( + ®.angularWalkVariance, ®.angularRateVariance, ®.magneticVariance, + ®.accelerationVariance); - return reg; + return reg; } -void VnSensor::writeFilterMeasurementsVarianceParameters(FilterMeasurementsVarianceParametersRegister &fields, bool waitForReply) +void VnSensor::writeFilterMeasurementsVarianceParameters( + FilterMeasurementsVarianceParametersRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteFilterMeasurementsVarianceParameters(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.angularWalkVariance, fields.angularRateVariance, fields.magneticVariance, fields.accelerationVariance); + size_t length = Packet::genWriteFilterMeasurementsVarianceParameters( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.angularWalkVariance, + fields.angularRateVariance, fields.magneticVariance, fields.accelerationVariance); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeFilterMeasurementsVarianceParameters( - const float &angularWalkVariance, - const vec3f &angularRateVariance, - const vec3f &magneticVariance, - const vec3f &accelerationVariance, - bool waitForReply) + const float & angularWalkVariance, const vec3f & angularRateVariance, + const vec3f & magneticVariance, const vec3f & accelerationVariance, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteFilterMeasurementsVarianceParameters( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - angularWalkVariance, - angularRateVariance, - magneticVariance, - accelerationVariance); + size_t length = Packet::genWriteFilterMeasurementsVarianceParameters( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), angularWalkVariance, angularRateVariance, + magneticVariance, accelerationVariance); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } MagnetometerCompensationRegister VnSensor::readMagnetometerCompensation() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadMagnetometerCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadMagnetometerCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - MagnetometerCompensationRegister reg; - response.parseMagnetometerCompensation( - ®.c, - ®.b); + MagnetometerCompensationRegister reg; + response.parseMagnetometerCompensation(®.c, ®.b); - return reg; + return reg; } -void VnSensor::writeMagnetometerCompensation(MagnetometerCompensationRegister &fields, bool waitForReply) +void VnSensor::writeMagnetometerCompensation( + MagnetometerCompensationRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteMagnetometerCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.c, fields.b); + size_t length = Packet::genWriteMagnetometerCompensation( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.c, fields.b); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } -void VnSensor::writeMagnetometerCompensation( - const mat3f &c, - const vec3f &b, - bool waitForReply) +void VnSensor::writeMagnetometerCompensation(const mat3f & c, const vec3f & b, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteMagnetometerCompensation( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - c, - b); + size_t length = Packet::genWriteMagnetometerCompensation( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), c, b); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } FilterActiveTuningParametersRegister VnSensor::readFilterActiveTuningParameters() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadFilterActiveTuningParameters(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadFilterActiveTuningParameters( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - FilterActiveTuningParametersRegister reg; - response.parseFilterActiveTuningParameters( - ®.magneticDisturbanceGain, - ®.accelerationDisturbanceGain, - ®.magneticDisturbanceMemory, - ®.accelerationDisturbanceMemory); + FilterActiveTuningParametersRegister reg; + response.parseFilterActiveTuningParameters( + ®.magneticDisturbanceGain, ®.accelerationDisturbanceGain, ®.magneticDisturbanceMemory, + ®.accelerationDisturbanceMemory); - return reg; + return reg; } -void VnSensor::writeFilterActiveTuningParameters(FilterActiveTuningParametersRegister &fields, bool waitForReply) +void VnSensor::writeFilterActiveTuningParameters( + FilterActiveTuningParametersRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteFilterActiveTuningParameters(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.magneticDisturbanceGain, fields.accelerationDisturbanceGain, fields.magneticDisturbanceMemory, fields.accelerationDisturbanceMemory); + size_t length = Packet::genWriteFilterActiveTuningParameters( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.magneticDisturbanceGain, + fields.accelerationDisturbanceGain, fields.magneticDisturbanceMemory, + fields.accelerationDisturbanceMemory); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeFilterActiveTuningParameters( - const float &magneticDisturbanceGain, - const float &accelerationDisturbanceGain, - const float &magneticDisturbanceMemory, - const float &accelerationDisturbanceMemory, - bool waitForReply) + const float & magneticDisturbanceGain, const float & accelerationDisturbanceGain, + const float & magneticDisturbanceMemory, const float & accelerationDisturbanceMemory, + bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteFilterActiveTuningParameters( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - magneticDisturbanceGain, - accelerationDisturbanceGain, - magneticDisturbanceMemory, - accelerationDisturbanceMemory); + size_t length = Packet::genWriteFilterActiveTuningParameters( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), magneticDisturbanceGain, + accelerationDisturbanceGain, magneticDisturbanceMemory, accelerationDisturbanceMemory); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } AccelerationCompensationRegister VnSensor::readAccelerationCompensation() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadAccelerationCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadAccelerationCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - AccelerationCompensationRegister reg; - response.parseAccelerationCompensation( - ®.c, - ®.b); + AccelerationCompensationRegister reg; + response.parseAccelerationCompensation(®.c, ®.b); - return reg; + return reg; } -void VnSensor::writeAccelerationCompensation(AccelerationCompensationRegister &fields, bool waitForReply) +void VnSensor::writeAccelerationCompensation( + AccelerationCompensationRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteAccelerationCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.c, fields.b); + size_t length = Packet::genWriteAccelerationCompensation( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.c, fields.b); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } -void VnSensor::writeAccelerationCompensation( - const mat3f &c, - const vec3f &b, - bool waitForReply) +void VnSensor::writeAccelerationCompensation(const mat3f & c, const vec3f & b, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteAccelerationCompensation( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - c, - b); + size_t length = Packet::genWriteAccelerationCompensation( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), c, b); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } mat3f VnSensor::readReferenceFrameRotation() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadReferenceFrameRotation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadReferenceFrameRotation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - mat3f reg; - response.parseReferenceFrameRotation(®); + mat3f reg; + response.parseReferenceFrameRotation(®); - return reg; + return reg; } -void VnSensor::writeReferenceFrameRotation(const mat3f &c, bool waitForReply) +void VnSensor::writeReferenceFrameRotation(const mat3f & c, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteReferenceFrameRotation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), c); + size_t length = + Packet::genWriteReferenceFrameRotation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), c); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } -YawPitchRollMagneticAccelerationAndAngularRatesRegister VnSensor::readYawPitchRollMagneticAccelerationAndAngularRates() +YawPitchRollMagneticAccelerationAndAngularRatesRegister +VnSensor::readYawPitchRollMagneticAccelerationAndAngularRates() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadYawPitchRollMagneticAccelerationAndAngularRates(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadYawPitchRollMagneticAccelerationAndAngularRates( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - YawPitchRollMagneticAccelerationAndAngularRatesRegister reg; - response.parseYawPitchRollMagneticAccelerationAndAngularRates( - ®.yawPitchRoll, - ®.mag, - ®.accel, - ®.gyro); + YawPitchRollMagneticAccelerationAndAngularRatesRegister reg; + response.parseYawPitchRollMagneticAccelerationAndAngularRates( + ®.yawPitchRoll, ®.mag, ®.accel, ®.gyro); - return reg; + return reg; } CommunicationProtocolControlRegister VnSensor::readCommunicationProtocolControl() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadCommunicationProtocolControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadCommunicationProtocolControl( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - CommunicationProtocolControlRegister reg; - uint8_t serialCount; - uint8_t serialStatus; - uint8_t spiCount; - uint8_t spiStatus; - uint8_t serialChecksum; - uint8_t spiChecksum; - uint8_t errorMode; - response.parseCommunicationProtocolControl( - &serialCount, - &serialStatus, - &spiCount, - &spiStatus, - &serialChecksum, - &spiChecksum, - &errorMode); + CommunicationProtocolControlRegister reg; + uint8_t serialCount; + uint8_t serialStatus; + uint8_t spiCount; + uint8_t spiStatus; + uint8_t serialChecksum; + uint8_t spiChecksum; + uint8_t errorMode; + response.parseCommunicationProtocolControl( + &serialCount, &serialStatus, &spiCount, &spiStatus, &serialChecksum, &spiChecksum, &errorMode); - reg.serialCount = static_cast(serialCount); - reg.serialStatus = static_cast(serialStatus); - reg.spiCount = static_cast(spiCount); - reg.spiStatus = static_cast(spiStatus); - reg.serialChecksum = static_cast(serialChecksum); - reg.spiChecksum = static_cast(spiChecksum); - reg.errorMode = static_cast(errorMode); + reg.serialCount = static_cast(serialCount); + reg.serialStatus = static_cast(serialStatus); + reg.spiCount = static_cast(spiCount); + reg.spiStatus = static_cast(spiStatus); + reg.serialChecksum = static_cast(serialChecksum); + reg.spiChecksum = static_cast(spiChecksum); + reg.errorMode = static_cast(errorMode); - return reg; + return reg; } -void VnSensor::writeCommunicationProtocolControl(CommunicationProtocolControlRegister &fields, bool waitForReply) +void VnSensor::writeCommunicationProtocolControl( + CommunicationProtocolControlRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteCommunicationProtocolControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.serialCount, fields.serialStatus, fields.spiCount, fields.spiStatus, fields.serialChecksum, fields.spiChecksum, fields.errorMode); + size_t length = Packet::genWriteCommunicationProtocolControl( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.serialCount, fields.serialStatus, + fields.spiCount, fields.spiStatus, fields.serialChecksum, fields.spiChecksum, fields.errorMode); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeCommunicationProtocolControl( - CountMode serialCount, - StatusMode serialStatus, - CountMode spiCount, - StatusMode spiStatus, - ChecksumMode serialChecksum, - ChecksumMode spiChecksum, - ErrorMode errorMode, - bool waitForReply) -{ - char toSend[256]; - - size_t length = Packet::genWriteCommunicationProtocolControl( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - serialCount, - serialStatus, - spiCount, - spiStatus, - serialChecksum, - spiChecksum, - errorMode); - - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + CountMode serialCount, StatusMode serialStatus, CountMode spiCount, StatusMode spiStatus, + ChecksumMode serialChecksum, ChecksumMode spiChecksum, ErrorMode errorMode, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteCommunicationProtocolControl( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), serialCount, serialStatus, spiCount, + spiStatus, serialChecksum, spiChecksum, errorMode); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } SynchronizationControlRegister VnSensor::readSynchronizationControl() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadSynchronizationControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadSynchronizationControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - SynchronizationControlRegister reg; - uint8_t syncInMode; - uint8_t syncInEdge; - uint8_t syncOutMode; - uint8_t syncOutPolarity; - response.parseSynchronizationControl( - &syncInMode, - &syncInEdge, - ®.syncInSkipFactor, - &syncOutMode, - &syncOutPolarity, - ®.syncOutSkipFactor, - ®.syncOutPulseWidth); + SynchronizationControlRegister reg; + uint8_t syncInMode; + uint8_t syncInEdge; + uint8_t syncOutMode; + uint8_t syncOutPolarity; + response.parseSynchronizationControl( + &syncInMode, &syncInEdge, ®.syncInSkipFactor, &syncOutMode, &syncOutPolarity, + ®.syncOutSkipFactor, ®.syncOutPulseWidth); - reg.syncInMode = static_cast(syncInMode); - reg.syncInEdge = static_cast(syncInEdge); - reg.syncOutMode = static_cast(syncOutMode); - reg.syncOutPolarity = static_cast(syncOutPolarity); + reg.syncInMode = static_cast(syncInMode); + reg.syncInEdge = static_cast(syncInEdge); + reg.syncOutMode = static_cast(syncOutMode); + reg.syncOutPolarity = static_cast(syncOutPolarity); - return reg; + return reg; } -void VnSensor::writeSynchronizationControl(SynchronizationControlRegister &fields, bool waitForReply) +void VnSensor::writeSynchronizationControl( + SynchronizationControlRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteSynchronizationControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.syncInMode, fields.syncInEdge, fields.syncInSkipFactor, fields.syncOutMode, fields.syncOutPolarity, fields.syncOutSkipFactor, fields.syncOutPulseWidth); + size_t length = Packet::genWriteSynchronizationControl( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.syncInMode, fields.syncInEdge, + fields.syncInSkipFactor, fields.syncOutMode, fields.syncOutPolarity, fields.syncOutSkipFactor, + fields.syncOutPulseWidth); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeSynchronizationControl( - SyncInMode syncInMode, - SyncInEdge syncInEdge, - const uint16_t &syncInSkipFactor, - SyncOutMode syncOutMode, - SyncOutPolarity syncOutPolarity, - const uint16_t &syncOutSkipFactor, - const uint32_t &syncOutPulseWidth, - bool waitForReply) -{ - char toSend[256]; - - size_t length = Packet::genWriteSynchronizationControl( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - syncInMode, - syncInEdge, - syncInSkipFactor, - syncOutMode, - syncOutPolarity, - syncOutSkipFactor, - syncOutPulseWidth); - - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + SyncInMode syncInMode, SyncInEdge syncInEdge, const uint16_t & syncInSkipFactor, + SyncOutMode syncOutMode, SyncOutPolarity syncOutPolarity, const uint16_t & syncOutSkipFactor, + const uint32_t & syncOutPulseWidth, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteSynchronizationControl( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), syncInMode, syncInEdge, syncInSkipFactor, + syncOutMode, syncOutPolarity, syncOutSkipFactor, syncOutPulseWidth); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } SynchronizationStatusRegister VnSensor::readSynchronizationStatus() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadSynchronizationStatus(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadSynchronizationStatus(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - SynchronizationStatusRegister reg; - response.parseSynchronizationStatus( - ®.syncInCount, - ®.syncInTime, - ®.syncOutCount); + SynchronizationStatusRegister reg; + response.parseSynchronizationStatus(®.syncInCount, ®.syncInTime, ®.syncOutCount); - return reg; + return reg; } -void VnSensor::writeSynchronizationStatus(SynchronizationStatusRegister &fields, bool waitForReply) +void VnSensor::writeSynchronizationStatus(SynchronizationStatusRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteSynchronizationStatus(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.syncInCount, fields.syncInTime, fields.syncOutCount); + size_t length = Packet::genWriteSynchronizationStatus( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.syncInCount, fields.syncInTime, + fields.syncOutCount); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeSynchronizationStatus( - const uint32_t &syncInCount, - const uint32_t &syncInTime, - const uint32_t &syncOutCount, - bool waitForReply) + const uint32_t & syncInCount, const uint32_t & syncInTime, const uint32_t & syncOutCount, + bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteSynchronizationStatus( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - syncInCount, - syncInTime, - syncOutCount); + size_t length = Packet::genWriteSynchronizationStatus( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), syncInCount, syncInTime, syncOutCount); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } FilterBasicControlRegister VnSensor::readFilterBasicControl() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadFilterBasicControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadFilterBasicControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - FilterBasicControlRegister reg; - uint8_t magMode; - uint8_t extMagMode; - uint8_t extAccMode; - uint8_t extGyroMode; - response.parseFilterBasicControl( - &magMode, - &extMagMode, - &extAccMode, - &extGyroMode, - ®.gyroLimit); + FilterBasicControlRegister reg; + uint8_t magMode; + uint8_t extMagMode; + uint8_t extAccMode; + uint8_t extGyroMode; + response.parseFilterBasicControl( + &magMode, &extMagMode, &extAccMode, &extGyroMode, ®.gyroLimit); - reg.magMode = static_cast(magMode); - reg.extMagMode = static_cast(extMagMode); - reg.extAccMode = static_cast(extAccMode); - reg.extGyroMode = static_cast(extGyroMode); + reg.magMode = static_cast(magMode); + reg.extMagMode = static_cast(extMagMode); + reg.extAccMode = static_cast(extAccMode); + reg.extGyroMode = static_cast(extGyroMode); - return reg; + return reg; } -void VnSensor::writeFilterBasicControl(FilterBasicControlRegister &fields, bool waitForReply) +void VnSensor::writeFilterBasicControl(FilterBasicControlRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteFilterBasicControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.magMode, fields.extMagMode, fields.extAccMode, fields.extGyroMode, fields.gyroLimit); + size_t length = Packet::genWriteFilterBasicControl( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.magMode, fields.extMagMode, + fields.extAccMode, fields.extGyroMode, fields.gyroLimit); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeFilterBasicControl( - MagneticMode magMode, - ExternalSensorMode extMagMode, - ExternalSensorMode extAccMode, - ExternalSensorMode extGyroMode, - const vec3f &gyroLimit, - bool waitForReply) + MagneticMode magMode, ExternalSensorMode extMagMode, ExternalSensorMode extAccMode, + ExternalSensorMode extGyroMode, const vec3f & gyroLimit, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteFilterBasicControl( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - magMode, - extMagMode, - extAccMode, - extGyroMode, - gyroLimit); + size_t length = Packet::genWriteFilterBasicControl( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), magMode, extMagMode, extAccMode, + extGyroMode, gyroLimit); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } HeaveConfigurationRegister VnSensor::readHeaveConfiguration() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadHeaveConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadHeaveConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - HeaveConfigurationRegister reg; - response.parseHeaveConfiguration( - ®.initialWavePeriod, - ®.initialWaveAmplitude, - ®.maxWavePeriod, - ®.minWaveAmplitude, - ®.delayedHeaveCutoffFreq, - ®.heaveCutoffFreq, - ®.heaveRateCutoffFreq); + HeaveConfigurationRegister reg; + response.parseHeaveConfiguration( + ®.initialWavePeriod, ®.initialWaveAmplitude, ®.maxWavePeriod, ®.minWaveAmplitude, + ®.delayedHeaveCutoffFreq, ®.heaveCutoffFreq, ®.heaveRateCutoffFreq); - return reg; + return reg; } -void VnSensor::writeHeaveConfiguration(HeaveConfigurationRegister &fields, bool waitForReply) +void VnSensor::writeHeaveConfiguration(HeaveConfigurationRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteHeaveConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), - fields.initialWavePeriod, - fields.initialWaveAmplitude, - fields.maxWavePeriod, - fields.minWaveAmplitude, - fields.delayedHeaveCutoffFreq, - fields.heaveCutoffFreq, - fields.heaveRateCutoffFreq); + size_t length = Packet::genWriteHeaveConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.initialWavePeriod, + fields.initialWaveAmplitude, fields.maxWavePeriod, fields.minWaveAmplitude, + fields.delayedHeaveCutoffFreq, fields.heaveCutoffFreq, fields.heaveRateCutoffFreq); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeHeaveConfiguration( - const float &initialWavePeriod, - const float &initialWaveAmplitude, - const float &maxWavePeriod, - const float &minWaveAmplitude, - const float &delayedHeaveCutoffFreq, - const float &heaveCutoffFreq, - const float &heaveRateCutoffFreq, - bool waitForReply) -{ - char toSend[256]; - - size_t length = Packet::genWriteHeaveConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), - initialWavePeriod, - initialWaveAmplitude, - maxWavePeriod, - minWaveAmplitude, - delayedHeaveCutoffFreq, - heaveCutoffFreq, - heaveRateCutoffFreq); - - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + const float & initialWavePeriod, const float & initialWaveAmplitude, const float & maxWavePeriod, + const float & minWaveAmplitude, const float & delayedHeaveCutoffFreq, + const float & heaveCutoffFreq, const float & heaveRateCutoffFreq, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteHeaveConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), initialWavePeriod, initialWaveAmplitude, + maxWavePeriod, minWaveAmplitude, delayedHeaveCutoffFreq, heaveCutoffFreq, heaveRateCutoffFreq); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } VpeBasicControlRegister VnSensor::readVpeBasicControl() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadVpeBasicControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadVpeBasicControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - VpeBasicControlRegister reg; - uint8_t enable; - uint8_t headingMode; - uint8_t filteringMode; - uint8_t tuningMode; - response.parseVpeBasicControl( - &enable, - &headingMode, - &filteringMode, - &tuningMode); + VpeBasicControlRegister reg; + uint8_t enable; + uint8_t headingMode; + uint8_t filteringMode; + uint8_t tuningMode; + response.parseVpeBasicControl(&enable, &headingMode, &filteringMode, &tuningMode); - reg.enable = static_cast(enable); - reg.headingMode = static_cast(headingMode); - reg.filteringMode = static_cast(filteringMode); - reg.tuningMode = static_cast(tuningMode); + reg.enable = static_cast(enable); + reg.headingMode = static_cast(headingMode); + reg.filteringMode = static_cast(filteringMode); + reg.tuningMode = static_cast(tuningMode); - return reg; + return reg; } -void VnSensor::writeVpeBasicControl(VpeBasicControlRegister &fields, bool waitForReply) +void VnSensor::writeVpeBasicControl(VpeBasicControlRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteVpeBasicControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.enable, fields.headingMode, fields.filteringMode, fields.tuningMode); + size_t length = Packet::genWriteVpeBasicControl( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.enable, fields.headingMode, + fields.filteringMode, fields.tuningMode); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeVpeBasicControl( - VpeEnable enable, - HeadingMode headingMode, - VpeMode filteringMode, - VpeMode tuningMode, - bool waitForReply) + VpeEnable enable, HeadingMode headingMode, VpeMode filteringMode, VpeMode tuningMode, + bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteVpeBasicControl( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - enable, - headingMode, - filteringMode, - tuningMode); + size_t length = Packet::genWriteVpeBasicControl( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), enable, headingMode, filteringMode, + tuningMode); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } VpeMagnetometerBasicTuningRegister VnSensor::readVpeMagnetometerBasicTuning() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadVpeMagnetometerBasicTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadVpeMagnetometerBasicTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - VpeMagnetometerBasicTuningRegister reg; - response.parseVpeMagnetometerBasicTuning( - ®.baseTuning, - ®.adaptiveTuning, - ®.adaptiveFiltering); + VpeMagnetometerBasicTuningRegister reg; + response.parseVpeMagnetometerBasicTuning( + ®.baseTuning, ®.adaptiveTuning, ®.adaptiveFiltering); - return reg; + return reg; } -void VnSensor::writeVpeMagnetometerBasicTuning(VpeMagnetometerBasicTuningRegister &fields, bool waitForReply) +void VnSensor::writeVpeMagnetometerBasicTuning( + VpeMagnetometerBasicTuningRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteVpeMagnetometerBasicTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.baseTuning, fields.adaptiveTuning, fields.adaptiveFiltering); + size_t length = Packet::genWriteVpeMagnetometerBasicTuning( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.baseTuning, fields.adaptiveTuning, + fields.adaptiveFiltering); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeVpeMagnetometerBasicTuning( - const vec3f &baseTuning, - const vec3f &adaptiveTuning, - const vec3f &adaptiveFiltering, - bool waitForReply) + const vec3f & baseTuning, const vec3f & adaptiveTuning, const vec3f & adaptiveFiltering, + bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteVpeMagnetometerBasicTuning( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - baseTuning, - adaptiveTuning, - adaptiveFiltering); + size_t length = Packet::genWriteVpeMagnetometerBasicTuning( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), baseTuning, adaptiveTuning, + adaptiveFiltering); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } VpeMagnetometerAdvancedTuningRegister VnSensor::readVpeMagnetometerAdvancedTuning() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadVpeMagnetometerAdvancedTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadVpeMagnetometerAdvancedTuning( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - VpeMagnetometerAdvancedTuningRegister reg; - response.parseVpeMagnetometerAdvancedTuning( - ®.minFiltering, - ®.maxFiltering, - ®.maxAdaptRate, - ®.disturbanceWindow, - ®.maxTuning); + VpeMagnetometerAdvancedTuningRegister reg; + response.parseVpeMagnetometerAdvancedTuning( + ®.minFiltering, ®.maxFiltering, ®.maxAdaptRate, ®.disturbanceWindow, + ®.maxTuning); - return reg; + return reg; } -void VnSensor::writeVpeMagnetometerAdvancedTuning(VpeMagnetometerAdvancedTuningRegister &fields, bool waitForReply) +void VnSensor::writeVpeMagnetometerAdvancedTuning( + VpeMagnetometerAdvancedTuningRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteVpeMagnetometerAdvancedTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.minFiltering, fields.maxFiltering, fields.maxAdaptRate, fields.disturbanceWindow, fields.maxTuning); + size_t length = Packet::genWriteVpeMagnetometerAdvancedTuning( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.minFiltering, fields.maxFiltering, + fields.maxAdaptRate, fields.disturbanceWindow, fields.maxTuning); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeVpeMagnetometerAdvancedTuning( - const vec3f &minFiltering, - const vec3f &maxFiltering, - const float &maxAdaptRate, - const float &disturbanceWindow, - const float &maxTuning, - bool waitForReply) + const vec3f & minFiltering, const vec3f & maxFiltering, const float & maxAdaptRate, + const float & disturbanceWindow, const float & maxTuning, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteVpeMagnetometerAdvancedTuning( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - minFiltering, - maxFiltering, - maxAdaptRate, - disturbanceWindow, - maxTuning); + size_t length = Packet::genWriteVpeMagnetometerAdvancedTuning( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), minFiltering, maxFiltering, maxAdaptRate, + disturbanceWindow, maxTuning); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } VpeAccelerometerBasicTuningRegister VnSensor::readVpeAccelerometerBasicTuning() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadVpeAccelerometerBasicTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadVpeAccelerometerBasicTuning( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - VpeAccelerometerBasicTuningRegister reg; - response.parseVpeAccelerometerBasicTuning( - ®.baseTuning, - ®.adaptiveTuning, - ®.adaptiveFiltering); + VpeAccelerometerBasicTuningRegister reg; + response.parseVpeAccelerometerBasicTuning( + ®.baseTuning, ®.adaptiveTuning, ®.adaptiveFiltering); - return reg; + return reg; } -void VnSensor::writeVpeAccelerometerBasicTuning(VpeAccelerometerBasicTuningRegister &fields, bool waitForReply) +void VnSensor::writeVpeAccelerometerBasicTuning( + VpeAccelerometerBasicTuningRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteVpeAccelerometerBasicTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.baseTuning, fields.adaptiveTuning, fields.adaptiveFiltering); + size_t length = Packet::genWriteVpeAccelerometerBasicTuning( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.baseTuning, fields.adaptiveTuning, + fields.adaptiveFiltering); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeVpeAccelerometerBasicTuning( - const vec3f &baseTuning, - const vec3f &adaptiveTuning, - const vec3f &adaptiveFiltering, - bool waitForReply) + const vec3f & baseTuning, const vec3f & adaptiveTuning, const vec3f & adaptiveFiltering, + bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteVpeAccelerometerBasicTuning( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - baseTuning, - adaptiveTuning, - adaptiveFiltering); + size_t length = Packet::genWriteVpeAccelerometerBasicTuning( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), baseTuning, adaptiveTuning, + adaptiveFiltering); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } VpeAccelerometerAdvancedTuningRegister VnSensor::readVpeAccelerometerAdvancedTuning() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadVpeAccelerometerAdvancedTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadVpeAccelerometerAdvancedTuning( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - VpeAccelerometerAdvancedTuningRegister reg; - response.parseVpeAccelerometerAdvancedTuning( - ®.minFiltering, - ®.maxFiltering, - ®.maxAdaptRate, - ®.disturbanceWindow, - ®.maxTuning); + VpeAccelerometerAdvancedTuningRegister reg; + response.parseVpeAccelerometerAdvancedTuning( + ®.minFiltering, ®.maxFiltering, ®.maxAdaptRate, ®.disturbanceWindow, + ®.maxTuning); - return reg; + return reg; } -void VnSensor::writeVpeAccelerometerAdvancedTuning(VpeAccelerometerAdvancedTuningRegister &fields, bool waitForReply) +void VnSensor::writeVpeAccelerometerAdvancedTuning( + VpeAccelerometerAdvancedTuningRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteVpeAccelerometerAdvancedTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.minFiltering, fields.maxFiltering, fields.maxAdaptRate, fields.disturbanceWindow, fields.maxTuning); + size_t length = Packet::genWriteVpeAccelerometerAdvancedTuning( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.minFiltering, fields.maxFiltering, + fields.maxAdaptRate, fields.disturbanceWindow, fields.maxTuning); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeVpeAccelerometerAdvancedTuning( - const vec3f &minFiltering, - const vec3f &maxFiltering, - const float &maxAdaptRate, - const float &disturbanceWindow, - const float &maxTuning, - bool waitForReply) + const vec3f & minFiltering, const vec3f & maxFiltering, const float & maxAdaptRate, + const float & disturbanceWindow, const float & maxTuning, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteVpeAccelerometerAdvancedTuning( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - minFiltering, - maxFiltering, - maxAdaptRate, - disturbanceWindow, - maxTuning); + size_t length = Packet::genWriteVpeAccelerometerAdvancedTuning( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), minFiltering, maxFiltering, maxAdaptRate, + disturbanceWindow, maxTuning); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } VpeGyroBasicTuningRegister VnSensor::readVpeGyroBasicTuning() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadVpeGyroBasicTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadVpeGyroBasicTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - VpeGyroBasicTuningRegister reg; - response.parseVpeGyroBasicTuning( - ®.angularWalkVariance, - ®.baseTuning, - ®.adaptiveTuning); + VpeGyroBasicTuningRegister reg; + response.parseVpeGyroBasicTuning(®.angularWalkVariance, ®.baseTuning, ®.adaptiveTuning); - return reg; + return reg; } -void VnSensor::writeVpeGyroBasicTuning(VpeGyroBasicTuningRegister &fields, bool waitForReply) +void VnSensor::writeVpeGyroBasicTuning(VpeGyroBasicTuningRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteVpeGyroBasicTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.angularWalkVariance, fields.baseTuning, fields.adaptiveTuning); + size_t length = Packet::genWriteVpeGyroBasicTuning( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.angularWalkVariance, + fields.baseTuning, fields.adaptiveTuning); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeVpeGyroBasicTuning( - const vec3f &angularWalkVariance, - const vec3f &baseTuning, - const vec3f &adaptiveTuning, - bool waitForReply) + const vec3f & angularWalkVariance, const vec3f & baseTuning, const vec3f & adaptiveTuning, + bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteVpeGyroBasicTuning( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - angularWalkVariance, - baseTuning, - adaptiveTuning); + size_t length = Packet::genWriteVpeGyroBasicTuning( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), angularWalkVariance, baseTuning, + adaptiveTuning); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } vec3f VnSensor::readFilterStartupGyroBias() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadFilterStartupGyroBias(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadFilterStartupGyroBias(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - vec3f reg; - response.parseFilterStartupGyroBias(®); + vec3f reg; + response.parseFilterStartupGyroBias(®); - return reg; + return reg; } -void VnSensor::writeFilterStartupGyroBias(const vec3f &bias, bool waitForReply) +void VnSensor::writeFilterStartupGyroBias(const vec3f & bias, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteFilterStartupGyroBias(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), bias); + size_t length = Packet::genWriteFilterStartupGyroBias( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), bias); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } MagnetometerCalibrationControlRegister VnSensor::readMagnetometerCalibrationControl() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadMagnetometerCalibrationControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadMagnetometerCalibrationControl( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - MagnetometerCalibrationControlRegister reg; - uint8_t hsiMode; - uint8_t hsiOutput; - response.parseMagnetometerCalibrationControl( - &hsiMode, - &hsiOutput, - ®.convergeRate); + MagnetometerCalibrationControlRegister reg; + uint8_t hsiMode; + uint8_t hsiOutput; + response.parseMagnetometerCalibrationControl(&hsiMode, &hsiOutput, ®.convergeRate); - reg.hsiMode = static_cast(hsiMode); - reg.hsiOutput = static_cast(hsiOutput); + reg.hsiMode = static_cast(hsiMode); + reg.hsiOutput = static_cast(hsiOutput); - return reg; + return reg; } -void VnSensor::writeMagnetometerCalibrationControl(MagnetometerCalibrationControlRegister &fields, bool waitForReply) +void VnSensor::writeMagnetometerCalibrationControl( + MagnetometerCalibrationControlRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteMagnetometerCalibrationControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.hsiMode, fields.hsiOutput, fields.convergeRate); + size_t length = Packet::genWriteMagnetometerCalibrationControl( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.hsiMode, fields.hsiOutput, + fields.convergeRate); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeMagnetometerCalibrationControl( - HsiMode hsiMode, - HsiOutput hsiOutput, - const uint8_t &convergeRate, - bool waitForReply) + HsiMode hsiMode, HsiOutput hsiOutput, const uint8_t & convergeRate, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteMagnetometerCalibrationControl( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - hsiMode, - hsiOutput, - convergeRate); + size_t length = Packet::genWriteMagnetometerCalibrationControl( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), hsiMode, hsiOutput, convergeRate); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } CalculatedMagnetometerCalibrationRegister VnSensor::readCalculatedMagnetometerCalibration() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadCalculatedMagnetometerCalibration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadCalculatedMagnetometerCalibration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - CalculatedMagnetometerCalibrationRegister reg; - response.parseCalculatedMagnetometerCalibration( - ®.c, - ®.b); + CalculatedMagnetometerCalibrationRegister reg; + response.parseCalculatedMagnetometerCalibration(®.c, ®.b); - return reg; + return reg; } float VnSensor::readIndoorHeadingModeControl() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadIndoorHeadingModeControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadIndoorHeadingModeControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - float reg; - response.parseIndoorHeadingModeControl(®); + float reg; + response.parseIndoorHeadingModeControl(®); - return reg; + return reg; } -void VnSensor::writeIndoorHeadingModeControl(const float &maxRateError, bool waitForReply) +void VnSensor::writeIndoorHeadingModeControl(const float & maxRateError, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteIndoorHeadingModeControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), maxRateError); + size_t length = Packet::genWriteIndoorHeadingModeControl( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), maxRateError); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } vec3f VnSensor::readVelocityCompensationMeasurement() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadVelocityCompensationMeasurement(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadVelocityCompensationMeasurement( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - vec3f reg; - response.parseVelocityCompensationMeasurement(®); + vec3f reg; + response.parseVelocityCompensationMeasurement(®); - return reg; + return reg; } -void VnSensor::writeVelocityCompensationMeasurement(const vec3f &velocity, bool waitForReply) +void VnSensor::writeVelocityCompensationMeasurement(const vec3f & velocity, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteVelocityCompensationMeasurement(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), velocity); + size_t length = Packet::genWriteVelocityCompensationMeasurement( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), velocity); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } VelocityCompensationControlRegister VnSensor::readVelocityCompensationControl() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadVelocityCompensationControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadVelocityCompensationControl( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - VelocityCompensationControlRegister reg; - uint8_t mode; - response.parseVelocityCompensationControl( - &mode, - ®.velocityTuning, - ®.rateTuning); + VelocityCompensationControlRegister reg; + uint8_t mode; + response.parseVelocityCompensationControl(&mode, ®.velocityTuning, ®.rateTuning); - reg.mode = static_cast(mode); + reg.mode = static_cast(mode); - return reg; + return reg; } -void VnSensor::writeVelocityCompensationControl(VelocityCompensationControlRegister &fields, bool waitForReply) +void VnSensor::writeVelocityCompensationControl( + VelocityCompensationControlRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteVelocityCompensationControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.mode, fields.velocityTuning, fields.rateTuning); + size_t length = Packet::genWriteVelocityCompensationControl( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.mode, fields.velocityTuning, + fields.rateTuning); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeVelocityCompensationControl( - VelocityCompensationMode mode, - const float &velocityTuning, - const float &rateTuning, - bool waitForReply) + VelocityCompensationMode mode, const float & velocityTuning, const float & rateTuning, + bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteVelocityCompensationControl( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - mode, - velocityTuning, - rateTuning); + size_t length = Packet::genWriteVelocityCompensationControl( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), mode, velocityTuning, rateTuning); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } VelocityCompensationStatusRegister VnSensor::readVelocityCompensationStatus() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadVelocityCompensationStatus(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadVelocityCompensationStatus(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - VelocityCompensationStatusRegister reg; - response.parseVelocityCompensationStatus( - ®.x, - ®.xDot, - ®.accelOffset, - ®.omega); + VelocityCompensationStatusRegister reg; + response.parseVelocityCompensationStatus(®.x, ®.xDot, ®.accelOffset, ®.omega); - return reg; + return reg; } ImuMeasurementsRegister VnSensor::readImuMeasurements() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadImuMeasurements(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadImuMeasurements(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - ImuMeasurementsRegister reg; - response.parseImuMeasurements( - ®.mag, - ®.accel, - ®.gyro, - ®.temp, - ®.pressure); + ImuMeasurementsRegister reg; + response.parseImuMeasurements(®.mag, ®.accel, ®.gyro, ®.temp, ®.pressure); - return reg; + return reg; } GpsConfigurationRegister VnSensor::readGpsConfiguration() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadGpsConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadGpsConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - GpsConfigurationRegister reg; - uint8_t mode; - uint8_t ppsSource; - uint8_t rate; - uint8_t antPow; - response.parseGpsConfiguration( - &mode, - &ppsSource, - &rate, - &antPow); + GpsConfigurationRegister reg; + uint8_t mode; + uint8_t ppsSource; + uint8_t rate; + uint8_t antPow; + response.parseGpsConfiguration(&mode, &ppsSource, &rate, &antPow); - reg.mode = static_cast(mode); - reg.ppsSource = static_cast(ppsSource); - reg.rate = static_cast(rate); - reg.antPow = static_cast(antPow); + reg.mode = static_cast(mode); + reg.ppsSource = static_cast(ppsSource); + reg.rate = static_cast(rate); + reg.antPow = static_cast(antPow); - return reg; + return reg; } -void VnSensor::writeGpsConfiguration(GpsConfigurationRegister &fields, bool waitForReply) +void VnSensor::writeGpsConfiguration(GpsConfigurationRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteGpsConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.mode, fields.ppsSource, fields.rate, fields.antPow); + size_t length = Packet::genWriteGpsConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.mode, fields.ppsSource, + fields.rate, fields.antPow); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } -void VnSensor::writeGpsConfiguration( - GpsMode mode, - PpsSource ppsSource, - bool waitForReply) +void VnSensor::writeGpsConfiguration(GpsMode mode, PpsSource ppsSource, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteGpsConfiguration( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - mode, - ppsSource); + size_t length = Packet::genWriteGpsConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), mode, ppsSource); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeGpsConfiguration( - GpsMode mode, - PpsSource ppsSource, - GpsRate rate, - AntPower antPow, - bool waitForReply) + GpsMode mode, PpsSource ppsSource, GpsRate rate, AntPower antPow, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteGpsConfiguration( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - mode, - ppsSource, - rate, - antPow); + size_t length = Packet::genWriteGpsConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), mode, ppsSource, rate, antPow); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } vec3f VnSensor::readGpsAntennaOffset() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadGpsAntennaOffset(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadGpsAntennaOffset(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - vec3f reg; - response.parseGpsAntennaOffset(®); + vec3f reg; + response.parseGpsAntennaOffset(®); - return reg; + return reg; } -void VnSensor::writeGpsAntennaOffset(const vec3f &position, bool waitForReply) +void VnSensor::writeGpsAntennaOffset(const vec3f & position, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteGpsAntennaOffset(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), position); + size_t length = Packet::genWriteGpsAntennaOffset( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), position); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } GpsSolutionLlaRegister VnSensor::readGpsSolutionLla() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadGpsSolutionLla(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadGpsSolutionLla(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - GpsSolutionLlaRegister reg; - uint8_t gpsFix; - response.parseGpsSolutionLla( - ®.time, - ®.week, - &gpsFix, - ®.numSats, - ®.lla, - ®.nedVel, - ®.nedAcc, - ®.speedAcc, - ®.timeAcc); + GpsSolutionLlaRegister reg; + uint8_t gpsFix; + response.parseGpsSolutionLla( + ®.time, ®.week, &gpsFix, ®.numSats, ®.lla, ®.nedVel, ®.nedAcc, ®.speedAcc, + ®.timeAcc); - reg.gpsFix = static_cast(gpsFix); + reg.gpsFix = static_cast(gpsFix); - return reg; + return reg; } GpsSolutionEcefRegister VnSensor::readGpsSolutionEcef() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadGpsSolutionEcef(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadGpsSolutionEcef(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - GpsSolutionEcefRegister reg; - uint8_t gpsFix; - response.parseGpsSolutionEcef( - ®.tow, - ®.week, - &gpsFix, - ®.numSats, - ®.position, - ®.velocity, - ®.posAcc, - ®.speedAcc, - ®.timeAcc); + GpsSolutionEcefRegister reg; + uint8_t gpsFix; + response.parseGpsSolutionEcef( + ®.tow, ®.week, &gpsFix, ®.numSats, ®.position, ®.velocity, ®.posAcc, + ®.speedAcc, ®.timeAcc); - reg.gpsFix = static_cast(gpsFix); + reg.gpsFix = static_cast(gpsFix); - return reg; + return reg; } InsSolutionLlaRegister VnSensor::readInsSolutionLla() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadInsSolutionLla(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadInsSolutionLla(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - InsSolutionLlaRegister reg; - response.parseInsSolutionLla( - ®.time, - ®.week, - ®.status, - ®.yawPitchRoll, - ®.position, - ®.nedVel, - ®.attUncertainty, - ®.posUncertainty, - ®.velUncertainty); + InsSolutionLlaRegister reg; + response.parseInsSolutionLla( + ®.time, ®.week, ®.status, ®.yawPitchRoll, ®.position, ®.nedVel, + ®.attUncertainty, ®.posUncertainty, ®.velUncertainty); - return reg; + return reg; } InsSolutionEcefRegister VnSensor::readInsSolutionEcef() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadInsSolutionEcef(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadInsSolutionEcef(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - InsSolutionEcefRegister reg; - response.parseInsSolutionEcef( - ®.time, - ®.week, - ®.status, - ®.yawPitchRoll, - ®.position, - ®.velocity, - ®.attUncertainty, - ®.posUncertainty, - ®.velUncertainty); + InsSolutionEcefRegister reg; + response.parseInsSolutionEcef( + ®.time, ®.week, ®.status, ®.yawPitchRoll, ®.position, ®.velocity, + ®.attUncertainty, ®.posUncertainty, ®.velUncertainty); - return reg; + return reg; } InsAdvancedConfigurationRegister VnSensor::readInsAdvancedConfiguration() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadInsAdvancedConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadInsAdvancedConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - InsAdvancedConfigurationRegister reg; - uint8_t useMag; - uint8_t usePres; - uint8_t posAtt; - uint8_t velAtt; - uint8_t velBias; - uint8_t useFoam; - response.parseInsAdvancedConfiguration( - &useMag, - &usePres, - &posAtt, - &velAtt, - &velBias, - &useFoam, - ®.gpsCovType, - ®.velCount, - ®.velInit, - ®.moveOrigin, - ®.gpsTimeout, - ®.deltaLimitPos, - ®.deltaLimitVel, - ®.minPosUncertainty, - ®.minVelUncertainty); + InsAdvancedConfigurationRegister reg; + uint8_t useMag; + uint8_t usePres; + uint8_t posAtt; + uint8_t velAtt; + uint8_t velBias; + uint8_t useFoam; + response.parseInsAdvancedConfiguration( + &useMag, &usePres, &posAtt, &velAtt, &velBias, &useFoam, ®.gpsCovType, ®.velCount, + ®.velInit, ®.moveOrigin, ®.gpsTimeout, ®.deltaLimitPos, ®.deltaLimitVel, + ®.minPosUncertainty, ®.minVelUncertainty); - reg.useMag = useMag != 0; - reg.usePres = usePres != 0; - reg.posAtt = posAtt != 0; - reg.velAtt = velAtt != 0; - reg.velBias = velBias != 0; - reg.useFoam = static_cast(useFoam); + reg.useMag = useMag != 0; + reg.usePres = usePres != 0; + reg.posAtt = posAtt != 0; + reg.velAtt = velAtt != 0; + reg.velBias = velBias != 0; + reg.useFoam = static_cast(useFoam); - return reg; + return reg; } -void VnSensor::writeInsAdvancedConfiguration(InsAdvancedConfigurationRegister &fields, bool waitForReply) +void VnSensor::writeInsAdvancedConfiguration( + InsAdvancedConfigurationRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteInsAdvancedConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.useMag, fields.usePres, fields.posAtt, fields.velAtt, fields.velBias, fields.useFoam, fields.gpsCovType, fields.velCount, fields.velInit, fields.moveOrigin, fields.gpsTimeout, fields.deltaLimitPos, fields.deltaLimitVel, fields.minPosUncertainty, fields.minVelUncertainty); + size_t length = Packet::genWriteInsAdvancedConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.useMag, fields.usePres, + fields.posAtt, fields.velAtt, fields.velBias, fields.useFoam, fields.gpsCovType, + fields.velCount, fields.velInit, fields.moveOrigin, fields.gpsTimeout, fields.deltaLimitPos, + fields.deltaLimitVel, fields.minPosUncertainty, fields.minVelUncertainty); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeInsAdvancedConfiguration( - const uint8_t &useMag, - const uint8_t &usePres, - const uint8_t &posAtt, - const uint8_t &velAtt, - const uint8_t &velBias, - FoamInit useFoam, - const uint8_t &gpsCovType, - const uint8_t &velCount, - const float &velInit, - const float &moveOrigin, - const float &gpsTimeout, - const float &deltaLimitPos, - const float &deltaLimitVel, - const float &minPosUncertainty, - const float &minVelUncertainty, - bool waitForReply) -{ - char toSend[256]; - - size_t length = Packet::genWriteInsAdvancedConfiguration( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - useMag, - usePres, - posAtt, - velAtt, - velBias, - useFoam, - gpsCovType, - velCount, - velInit, - moveOrigin, - gpsTimeout, - deltaLimitPos, - deltaLimitVel, - minPosUncertainty, - minVelUncertainty); - - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + const uint8_t & useMag, const uint8_t & usePres, const uint8_t & posAtt, const uint8_t & velAtt, + const uint8_t & velBias, FoamInit useFoam, const uint8_t & gpsCovType, const uint8_t & velCount, + const float & velInit, const float & moveOrigin, const float & gpsTimeout, + const float & deltaLimitPos, const float & deltaLimitVel, const float & minPosUncertainty, + const float & minVelUncertainty, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteInsAdvancedConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), useMag, usePres, posAtt, velAtt, velBias, + useFoam, gpsCovType, velCount, velInit, moveOrigin, gpsTimeout, deltaLimitPos, deltaLimitVel, + minPosUncertainty, minVelUncertainty); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } InsStateLlaRegister VnSensor::readInsStateLla() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadInsStateLla(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadInsStateLla(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - InsStateLlaRegister reg; - response.parseInsStateLla( - ®.yawPitchRoll, - ®.position, - ®.velocity, - ®.accel, - ®.angularRate); + InsStateLlaRegister reg; + response.parseInsStateLla( + ®.yawPitchRoll, ®.position, ®.velocity, ®.accel, ®.angularRate); - return reg; + return reg; } InsStateEcefRegister VnSensor::readInsStateEcef() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadInsStateEcef(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadInsStateEcef(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - InsStateEcefRegister reg; - response.parseInsStateEcef( - ®.yawPitchRoll, - ®.position, - ®.velocity, - ®.accel, - ®.angularRate); + InsStateEcefRegister reg; + response.parseInsStateEcef( + ®.yawPitchRoll, ®.position, ®.velocity, ®.accel, ®.angularRate); - return reg; + return reg; } StartupFilterBiasEstimateRegister VnSensor::readStartupFilterBiasEstimate() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadStartupFilterBiasEstimate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadStartupFilterBiasEstimate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - StartupFilterBiasEstimateRegister reg; - response.parseStartupFilterBiasEstimate( - ®.gyroBias, - ®.accelBias, - ®.pressureBias); + StartupFilterBiasEstimateRegister reg; + response.parseStartupFilterBiasEstimate(®.gyroBias, ®.accelBias, ®.pressureBias); - return reg; + return reg; } -void VnSensor::writeStartupFilterBiasEstimate(StartupFilterBiasEstimateRegister &fields, bool waitForReply) +void VnSensor::writeStartupFilterBiasEstimate( + StartupFilterBiasEstimateRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteStartupFilterBiasEstimate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.gyroBias, fields.accelBias, fields.pressureBias); + size_t length = Packet::genWriteStartupFilterBiasEstimate( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.gyroBias, fields.accelBias, + fields.pressureBias); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeStartupFilterBiasEstimate( - const vec3f &gyroBias, - const vec3f &accelBias, - const float &pressureBias, - bool waitForReply) + const vec3f & gyroBias, const vec3f & accelBias, const float & pressureBias, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteStartupFilterBiasEstimate( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - gyroBias, - accelBias, - pressureBias); + size_t length = Packet::genWriteStartupFilterBiasEstimate( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), gyroBias, accelBias, pressureBias); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } DeltaThetaAndDeltaVelocityRegister VnSensor::readDeltaThetaAndDeltaVelocity() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadDeltaThetaAndDeltaVelocity(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadDeltaThetaAndDeltaVelocity(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - DeltaThetaAndDeltaVelocityRegister reg; - response.parseDeltaThetaAndDeltaVelocity( - ®.deltaTime, - ®.deltaTheta, - ®.deltaVelocity); + DeltaThetaAndDeltaVelocityRegister reg; + response.parseDeltaThetaAndDeltaVelocity(®.deltaTime, ®.deltaTheta, ®.deltaVelocity); - return reg; + return reg; } -DeltaThetaAndDeltaVelocityConfigurationRegister VnSensor::readDeltaThetaAndDeltaVelocityConfiguration() +DeltaThetaAndDeltaVelocityConfigurationRegister +VnSensor::readDeltaThetaAndDeltaVelocityConfiguration() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadDeltaThetaAndDeltaVelocityConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadDeltaThetaAndDeltaVelocityConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - DeltaThetaAndDeltaVelocityConfigurationRegister reg; - uint8_t integrationFrame; - uint8_t gyroCompensation; - uint8_t accelCompensation; - uint8_t earthRateCorrection; - response.parseDeltaThetaAndDeltaVelocityConfiguration( - &integrationFrame, - &gyroCompensation, - &accelCompensation, - &earthRateCorrection); + DeltaThetaAndDeltaVelocityConfigurationRegister reg; + uint8_t integrationFrame; + uint8_t gyroCompensation; + uint8_t accelCompensation; + uint8_t earthRateCorrection; + response.parseDeltaThetaAndDeltaVelocityConfiguration( + &integrationFrame, &gyroCompensation, &accelCompensation, &earthRateCorrection); - reg.integrationFrame = static_cast(integrationFrame); - reg.gyroCompensation = static_cast(gyroCompensation); - reg.accelCompensation = static_cast(accelCompensation); - reg.earthRateCorrection = static_cast(earthRateCorrection); + reg.integrationFrame = static_cast(integrationFrame); + reg.gyroCompensation = static_cast(gyroCompensation); + reg.accelCompensation = static_cast(accelCompensation); + reg.earthRateCorrection = static_cast(earthRateCorrection); - return reg; + return reg; } -void VnSensor::writeDeltaThetaAndDeltaVelocityConfiguration(DeltaThetaAndDeltaVelocityConfigurationRegister &fields, bool waitForReply) +void VnSensor::writeDeltaThetaAndDeltaVelocityConfiguration( + DeltaThetaAndDeltaVelocityConfigurationRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteDeltaThetaAndDeltaVelocityConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.integrationFrame, fields.gyroCompensation, fields.accelCompensation, fields.earthRateCorrection); + size_t length = Packet::genWriteDeltaThetaAndDeltaVelocityConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.integrationFrame, + fields.gyroCompensation, fields.accelCompensation, fields.earthRateCorrection); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeDeltaThetaAndDeltaVelocityConfiguration( - IntegrationFrame integrationFrame, - CompensationMode gyroCompensation, - AccCompensationMode accelCompensation, - bool waitForReply) + IntegrationFrame integrationFrame, CompensationMode gyroCompensation, + AccCompensationMode accelCompensation, bool waitForReply) { - writeDeltaThetaAndDeltaVelocityConfiguration( - integrationFrame, - gyroCompensation, - accelCompensation, - protocol::uart::EARTHRATECORR_NONE, - waitForReply); + writeDeltaThetaAndDeltaVelocityConfiguration( + integrationFrame, gyroCompensation, accelCompensation, protocol::uart::EARTHRATECORR_NONE, + waitForReply); } void VnSensor::writeDeltaThetaAndDeltaVelocityConfiguration( - IntegrationFrame integrationFrame, - CompensationMode gyroCompensation, - AccCompensationMode accelCompensation, - EarthRateCorrection earthRateCorrection, - bool waitForReply) + IntegrationFrame integrationFrame, CompensationMode gyroCompensation, + AccCompensationMode accelCompensation, EarthRateCorrection earthRateCorrection, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteDeltaThetaAndDeltaVelocityConfiguration( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - integrationFrame, - gyroCompensation, - accelCompensation, - earthRateCorrection); + size_t length = Packet::genWriteDeltaThetaAndDeltaVelocityConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), integrationFrame, gyroCompensation, + accelCompensation, earthRateCorrection); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } ReferenceVectorConfigurationRegister VnSensor::readReferenceVectorConfiguration() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadReferenceVectorConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadReferenceVectorConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - ReferenceVectorConfigurationRegister reg; - uint8_t useMagModel; - uint8_t useGravityModel; - response.parseReferenceVectorConfiguration( - &useMagModel, - &useGravityModel, - ®.recalcThreshold, - ®.year, - ®.position); + ReferenceVectorConfigurationRegister reg; + uint8_t useMagModel; + uint8_t useGravityModel; + response.parseReferenceVectorConfiguration( + &useMagModel, &useGravityModel, ®.recalcThreshold, ®.year, ®.position); - reg.useMagModel = useMagModel != 0; - reg.useGravityModel = useGravityModel != 0; + reg.useMagModel = useMagModel != 0; + reg.useGravityModel = useGravityModel != 0; - return reg; + return reg; } -void VnSensor::writeReferenceVectorConfiguration(ReferenceVectorConfigurationRegister &fields, bool waitForReply) +void VnSensor::writeReferenceVectorConfiguration( + ReferenceVectorConfigurationRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteReferenceVectorConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.useMagModel, fields.useGravityModel, fields.recalcThreshold, fields.year, fields.position); + size_t length = Packet::genWriteReferenceVectorConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.useMagModel, + fields.useGravityModel, fields.recalcThreshold, fields.year, fields.position); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeReferenceVectorConfiguration( - const uint8_t &useMagModel, - const uint8_t &useGravityModel, - const uint32_t &recalcThreshold, - const float &year, - const vec3d &position, - bool waitForReply) + const uint8_t & useMagModel, const uint8_t & useGravityModel, const uint32_t & recalcThreshold, + const float & year, const vec3d & position, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteReferenceVectorConfiguration( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - useMagModel, - useGravityModel, - recalcThreshold, - year, - position); + size_t length = Packet::genWriteReferenceVectorConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), useMagModel, useGravityModel, + recalcThreshold, year, position); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } GyroCompensationRegister VnSensor::readGyroCompensation() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadGyroCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadGyroCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - GyroCompensationRegister reg; - response.parseGyroCompensation( - ®.c, - ®.b); + GyroCompensationRegister reg; + response.parseGyroCompensation(®.c, ®.b); - return reg; + return reg; } -void VnSensor::writeGyroCompensation(GyroCompensationRegister &fields, bool waitForReply) +void VnSensor::writeGyroCompensation(GyroCompensationRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteGyroCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.c, fields.b); + size_t length = Packet::genWriteGyroCompensation( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.c, fields.b); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } -void VnSensor::writeGyroCompensation( - const mat3f &c, - const vec3f &b, - bool waitForReply) +void VnSensor::writeGyroCompensation(const mat3f & c, const vec3f & b, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteGyroCompensation( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - c, - b); + size_t length = + Packet::genWriteGyroCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), c, b); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } ImuFilteringConfigurationRegister VnSensor::readImuFilteringConfiguration() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadImuFilteringConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadImuFilteringConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - ImuFilteringConfigurationRegister reg; - uint8_t magFilterMode; - uint8_t accelFilterMode; - uint8_t gyroFilterMode; - uint8_t tempFilterMode; - uint8_t presFilterMode; - response.parseImuFilteringConfiguration( - ®.magWindowSize, - ®.accelWindowSize, - ®.gyroWindowSize, - ®.tempWindowSize, - ®.presWindowSize, - &magFilterMode, - &accelFilterMode, - &gyroFilterMode, - &tempFilterMode, - &presFilterMode); + ImuFilteringConfigurationRegister reg; + uint8_t magFilterMode; + uint8_t accelFilterMode; + uint8_t gyroFilterMode; + uint8_t tempFilterMode; + uint8_t presFilterMode; + response.parseImuFilteringConfiguration( + ®.magWindowSize, ®.accelWindowSize, ®.gyroWindowSize, ®.tempWindowSize, + ®.presWindowSize, &magFilterMode, &accelFilterMode, &gyroFilterMode, &tempFilterMode, + &presFilterMode); - reg.magFilterMode = static_cast(magFilterMode); - reg.accelFilterMode = static_cast(accelFilterMode); - reg.gyroFilterMode = static_cast(gyroFilterMode); - reg.tempFilterMode = static_cast(tempFilterMode); - reg.presFilterMode = static_cast(presFilterMode); + reg.magFilterMode = static_cast(magFilterMode); + reg.accelFilterMode = static_cast(accelFilterMode); + reg.gyroFilterMode = static_cast(gyroFilterMode); + reg.tempFilterMode = static_cast(tempFilterMode); + reg.presFilterMode = static_cast(presFilterMode); - return reg; + return reg; } -void VnSensor::writeImuFilteringConfiguration(ImuFilteringConfigurationRegister &fields, bool waitForReply) +void VnSensor::writeImuFilteringConfiguration( + ImuFilteringConfigurationRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteImuFilteringConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.magWindowSize, fields.accelWindowSize, fields.gyroWindowSize, fields.tempWindowSize, fields.presWindowSize, fields.magFilterMode, fields.accelFilterMode, fields.gyroFilterMode, fields.tempFilterMode, fields.presFilterMode); + size_t length = Packet::genWriteImuFilteringConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.magWindowSize, + fields.accelWindowSize, fields.gyroWindowSize, fields.tempWindowSize, fields.presWindowSize, + fields.magFilterMode, fields.accelFilterMode, fields.gyroFilterMode, fields.tempFilterMode, + fields.presFilterMode); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeImuFilteringConfiguration( - const uint16_t &magWindowSize, - const uint16_t &accelWindowSize, - const uint16_t &gyroWindowSize, - const uint16_t &tempWindowSize, - const uint16_t &presWindowSize, - FilterMode magFilterMode, - FilterMode accelFilterMode, - FilterMode gyroFilterMode, - FilterMode tempFilterMode, - FilterMode presFilterMode, - bool waitForReply) -{ - char toSend[256]; - - size_t length = Packet::genWriteImuFilteringConfiguration( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - magWindowSize, - accelWindowSize, - gyroWindowSize, - tempWindowSize, - presWindowSize, - magFilterMode, - accelFilterMode, - gyroFilterMode, - tempFilterMode, - presFilterMode); - - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + const uint16_t & magWindowSize, const uint16_t & accelWindowSize, const uint16_t & gyroWindowSize, + const uint16_t & tempWindowSize, const uint16_t & presWindowSize, FilterMode magFilterMode, + FilterMode accelFilterMode, FilterMode gyroFilterMode, FilterMode tempFilterMode, + FilterMode presFilterMode, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteImuFilteringConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), magWindowSize, accelWindowSize, + gyroWindowSize, tempWindowSize, presWindowSize, magFilterMode, accelFilterMode, gyroFilterMode, + tempFilterMode, presFilterMode); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } GpsCompassBaselineRegister VnSensor::readGpsCompassBaseline() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadGpsCompassBaseline(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadGpsCompassBaseline(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - GpsCompassBaselineRegister reg; - response.parseGpsCompassBaseline( - ®.position, - ®.uncertainty); + GpsCompassBaselineRegister reg; + response.parseGpsCompassBaseline(®.position, ®.uncertainty); - return reg; + return reg; } -void VnSensor::writeGpsCompassBaseline(GpsCompassBaselineRegister &fields, bool waitForReply) +void VnSensor::writeGpsCompassBaseline(GpsCompassBaselineRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteGpsCompassBaseline(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.position, fields.uncertainty); + size_t length = Packet::genWriteGpsCompassBaseline( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.position, fields.uncertainty); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeGpsCompassBaseline( - const vec3f &position, - const vec3f &uncertainty, - bool waitForReply) + const vec3f & position, const vec3f & uncertainty, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteGpsCompassBaseline( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - position, - uncertainty); + size_t length = Packet::genWriteGpsCompassBaseline( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), position, uncertainty); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } GpsCompassEstimatedBaselineRegister VnSensor::readGpsCompassEstimatedBaseline() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadGpsCompassEstimatedBaseline(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadGpsCompassEstimatedBaseline( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - GpsCompassEstimatedBaselineRegister reg; - uint8_t estBaselineUsed; - response.parseGpsCompassEstimatedBaseline( - &estBaselineUsed, - ®.numMeas, - ®.position, - ®.uncertainty); + GpsCompassEstimatedBaselineRegister reg; + uint8_t estBaselineUsed; + response.parseGpsCompassEstimatedBaseline( + &estBaselineUsed, ®.numMeas, ®.position, ®.uncertainty); - reg.estBaselineUsed = estBaselineUsed != 0; + reg.estBaselineUsed = estBaselineUsed != 0; - return reg; + return reg; } ImuRateConfigurationRegister VnSensor::readImuRateConfiguration() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadImuRateConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = + Packet::genReadImuRateConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - ImuRateConfigurationRegister reg; - response.parseImuRateConfiguration( - ®.imuRate, - ®.navDivisor, - ®.filterTargetRate, - ®.filterMinRate); + ImuRateConfigurationRegister reg; + response.parseImuRateConfiguration( + ®.imuRate, ®.navDivisor, ®.filterTargetRate, ®.filterMinRate); - return reg; + return reg; } -void VnSensor::writeImuRateConfiguration(ImuRateConfigurationRegister &fields, bool waitForReply) +void VnSensor::writeImuRateConfiguration(ImuRateConfigurationRegister & fields, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteImuRateConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.imuRate, fields.navDivisor, fields.filterTargetRate, fields.filterMinRate); + size_t length = Packet::genWriteImuRateConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.imuRate, fields.navDivisor, + fields.filterTargetRate, fields.filterMinRate); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } void VnSensor::writeImuRateConfiguration( - const uint16_t &imuRate, - const uint16_t &navDivisor, - const float &filterTargetRate, - const float &filterMinRate, - bool waitForReply) + const uint16_t & imuRate, const uint16_t & navDivisor, const float & filterTargetRate, + const float & filterMinRate, bool waitForReply) { - char toSend[256]; + char toSend[256]; - size_t length = Packet::genWriteImuRateConfiguration( - _pi->_sendErrorDetectionMode, - toSend, - sizeof(toSend), - imuRate, - navDivisor, - filterTargetRate, - filterMinRate); + size_t length = Packet::genWriteImuRateConfiguration( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend), imuRate, navDivisor, filterTargetRate, + filterMinRate); - Packet response; - _pi->transactionNoFinalize(toSend, length, waitForReply, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); } -YawPitchRollTrueBodyAccelerationAndAngularRatesRegister VnSensor::readYawPitchRollTrueBodyAccelerationAndAngularRates() +YawPitchRollTrueBodyAccelerationAndAngularRatesRegister +VnSensor::readYawPitchRollTrueBodyAccelerationAndAngularRates() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadYawPitchRollTrueBodyAccelerationAndAngularRates(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadYawPitchRollTrueBodyAccelerationAndAngularRates( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - YawPitchRollTrueBodyAccelerationAndAngularRatesRegister reg; - response.parseYawPitchRollTrueBodyAccelerationAndAngularRates( - ®.yawPitchRoll, - ®.bodyAccel, - ®.gyro); + YawPitchRollTrueBodyAccelerationAndAngularRatesRegister reg; + response.parseYawPitchRollTrueBodyAccelerationAndAngularRates( + ®.yawPitchRoll, ®.bodyAccel, ®.gyro); - return reg; + return reg; } -YawPitchRollTrueInertialAccelerationAndAngularRatesRegister VnSensor::readYawPitchRollTrueInertialAccelerationAndAngularRates() +YawPitchRollTrueInertialAccelerationAndAngularRatesRegister +VnSensor::readYawPitchRollTrueInertialAccelerationAndAngularRates() { - char toSend[17]; + char toSend[17]; - size_t length = Packet::genReadYawPitchRollTrueInertialAccelerationAndAngularRates(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + size_t length = Packet::genReadYawPitchRollTrueInertialAccelerationAndAngularRates( + _pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); - Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response); + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); - YawPitchRollTrueInertialAccelerationAndAngularRatesRegister reg; - response.parseYawPitchRollTrueInertialAccelerationAndAngularRates( - ®.yawPitchRoll, - ®.inertialAccel, - ®.gyro); + YawPitchRollTrueInertialAccelerationAndAngularRatesRegister reg; + response.parseYawPitchRollTrueInertialAccelerationAndAngularRates( + ®.yawPitchRoll, ®.inertialAccel, ®.gyro); - return reg; + return reg; } void VnSensor::switchProcessors(VnProcessorType processor, std::string model, std::string firmware) { - std::string switchCmd = ""; - - if (model.find("VN-300") != std::string::npos) - { - switch (processor) - { - case VNPROCESSOR_GPS: - if ((model.find("SMD") != std::string::npos) || (model.find("DEV") != std::string::npos)) - { - switchCmd = "$VNSPS,1,1,115200"; - } - else if (model.find("CR") != std::string::npos) - { - switchCmd = "$VNDBS,3,1"; - } - break; - } - } - else if ((model.find("VN-110") != std::string::npos) || (model.find("VN-210") != std::string::npos) || (model.find("VN-310") != std::string::npos)) - { - switch (processor) - { - case VNPROCESSOR_NAV: - switchCmd = "$VNSBL,0"; - break; - case VNPROCESSOR_GPS: - if ((model.find("VN-310") != std::string::npos) || (model.find("VN-210E") != std::string::npos)) - { - switchCmd = "$VNSBL,1"; - } - break; - case VNPROCESSOR_IMU: - switchCmd = "$VNSBL,2"; - break; - } - } - - if (switchCmd.empty()) - { - throw invalid_operation(); - } - - // Send switch command - char toSend[256]; + std::string switchCmd = ""; + + if (model.find("VN-300") != std::string::npos) { + switch (processor) { + case VNPROCESSOR_GPS: + if ((model.find("SMD") != std::string::npos) || (model.find("DEV") != std::string::npos)) { + switchCmd = "$VNSPS,1,1,115200"; + } else if (model.find("CR") != std::string::npos) { + switchCmd = "$VNDBS,3,1"; + } + break; + } + } else if ( + (model.find("VN-110") != std::string::npos) || (model.find("VN-210") != std::string::npos) || + (model.find("VN-310") != std::string::npos)) { + switch (processor) { + case VNPROCESSOR_NAV: + switchCmd = "$VNSBL,0"; + break; + case VNPROCESSOR_GPS: + if ( + (model.find("VN-310") != std::string::npos) || + (model.find("VN-210E") != std::string::npos)) { + switchCmd = "$VNSBL,1"; + } + break; + case VNPROCESSOR_IMU: + switchCmd = "$VNSBL,2"; + break; + } + } + + if (switchCmd.empty()) { + throw invalid_operation(); + } + + // Send switch command + char toSend[256]; #if VN_HAVE_SECURE_CRT - strcpy_s(toSend, 256, switchCmd.c_str()); + strcpy_s(toSend, 256, switchCmd.c_str()); #else - strcpy(toSend, switchCmd.c_str()); + strcpy(toSend, switchCmd.c_str()); #endif - size_t length = Packet::finalizeCommand(ERRORDETECTIONMODE_CRC, toSend, switchCmd.length()); - Packet response; + size_t length = Packet::finalizeCommand(ERRORDETECTIONMODE_CRC, toSend, switchCmd.length()); + Packet response; - _pi->transactionNoFinalize(toSend, length, true, &response, 6000, 6000); + _pi->transactionNoFinalize(toSend, length, true, &response, 6000, 6000); - // Wait 2 seconds to give the processor time to switch - Thread::sleepSec(2); + // Wait 2 seconds to give the processor time to switch + Thread::sleepSec(2); } void VnSensor::firmwareUpdate(int baudRate, std::string fileName) { - std::string record = ""; - - // Open firmware update file - openFirmwareUpdateFile(fileName); - - // Enter bootloader mode - firmwareUpdateMode(true); - - // Give the sensor time to reboot into bootloader mode - Thread::sleepSec(1); - - // Configure baudrate for firmware update - uint32_t navBaudRate = baudrate(); - setFirmwareUpdateBaudRate(baudRate); - - // Calibrate the bootloader by letting it calculate the current baudrate. - std::string bootloaderVersion = calibrateBootloader(); - //cout << bootloaderVersion << endl; - - // Send each record from the firmware update file one at a time - do - { - record = getNextFirwareUpdateRecord(); - if (!record.empty()) - { - record.erase(0, 1); // Remove the ':' from the beginning of the line. - writeFirmwareUpdateRecord(record, true); - } - } while (!record.empty()); - - // Close firmware update file - closeFirmwareUpdateFile(); - - // Reset baudrate - setFirmwareUpdateBaudRate(navBaudRate); - - // Exit bootloader mode. Just sleep for 10 seconds - Thread::sleepSec(10); - - // Do a reset - reset(true); - - // Give the sensor time to recover after the reset - Thread::sleepSec(2); + std::string record = ""; + + // Open firmware update file + openFirmwareUpdateFile(fileName); + + // Enter bootloader mode + firmwareUpdateMode(true); + + // Give the sensor time to reboot into bootloader mode + Thread::sleepSec(1); + + // Configure baudrate for firmware update + uint32_t navBaudRate = baudrate(); + setFirmwareUpdateBaudRate(baudRate); + + // Calibrate the bootloader by letting it calculate the current baudrate. + std::string bootloaderVersion = calibrateBootloader(); + //cout << bootloaderVersion << endl; + + // Send each record from the firmware update file one at a time + do { + record = getNextFirwareUpdateRecord(); + if (!record.empty()) { + record.erase(0, 1); // Remove the ':' from the beginning of the line. + writeFirmwareUpdateRecord(record, true); + } + } while (!record.empty()); + + // Close firmware update file + closeFirmwareUpdateFile(); + + // Reset baudrate + setFirmwareUpdateBaudRate(navBaudRate); + + // Exit bootloader mode. Just sleep for 10 seconds + Thread::sleepSec(10); + + // Do a reset + reset(true); + + // Give the sensor time to recover after the reset + Thread::sleepSec(2); } void VnSensor::setFirmwareUpdateBaudRate(int baudRate) { - _pi->pSerialPort->changeBaudrate(baudRate); + _pi->pSerialPort->changeBaudrate(baudRate); } -void VnSensor::writeFirmwareUpdateRecord(const string& record, bool waitForReply) +void VnSensor::writeFirmwareUpdateRecord(const string & record, bool waitForReply) { - char toSend[MAXFIRMWAREUPDATERECORDSIZE +12]; + char toSend[MAXFIRMWAREUPDATERECORDSIZE + 12]; - size_t length = Packet::genWriteFirmwareUpdateRecord(ERRORDETECTIONMODE_CRC, toSend, sizeof(toSend), record.c_str()); + size_t length = Packet::genWriteFirmwareUpdateRecord( + ERRORDETECTIONMODE_CRC, toSend, sizeof(toSend), record.c_str()); - Packet response; + Packet response; RESEND: - _pi->_filteringBootloaderResponses = true; /*Enable Bootloader Filtering just before sending the bootloader command */ - _pi->transactionNoFinalize(toSend, length, waitForReply, &response, 6000, 6000); - - BootloaderError responseCode = (BootloaderError)std::stoi(response.datastr().substr(7,2)); + _pi->_filteringBootloaderResponses = + true; /*Enable Bootloader Filtering just before sending the bootloader command */ + _pi->transactionNoFinalize(toSend, length, waitForReply, &response, 6000, 6000); + BootloaderError responseCode = (BootloaderError)std::stoi(response.datastr().substr(7, 2)); - if (responseCode == BLE_COMM_ERROR) - { - // This is the only case where we will try again - goto RESEND; - } + if (responseCode == BLE_COMM_ERROR) { + // This is the only case where we will try again + goto RESEND; + } - if (responseCode != BLE_NONE) - { - // Abort - cout << vn::protocol::uart::str(responseCode) << endl; - throw unknown_error(); - } + if (responseCode != BLE_NONE) { + // Abort + cout << vn::protocol::uart::str(responseCode) << endl; + throw unknown_error(); + } } - std::string VnSensor::calibrateBootloader() { - char baudratetest[] = " "; /* Only need 8, but doubling it just to be safe */ - char bootloaderVersion[64]; - memset(bootloaderVersion, 0, 64); - uint16_t responseTimeoutMs = 500; - uint16_t retransmitDelayMs = 500; + char baudratetest[] = " "; /* Only need 8, but doubling it just to be safe */ + char bootloaderVersion[64]; + memset(bootloaderVersion, 0, 64); + uint16_t responseTimeoutMs = 500; + uint16_t retransmitDelayMs = 500; - Packet response; - _pi->_filteringBootloaderResponses = true; /*Enable Bootloader Filtering just before sending the bootloader command */ - _pi->transactionNoFinalize(baudratetest, sizeof(baudratetest)-2, true, &response, 7000, 7000); + Packet response; + _pi->_filteringBootloaderResponses = + true; /*Enable Bootloader Filtering just before sending the bootloader command */ + _pi->transactionNoFinalize(baudratetest, sizeof(baudratetest) - 2, true, &response, 7000, 7000); - std::string version = response.datastr(); - return version; + std::string version = response.datastr(); + return version; } std::string VnSensor::getNextFirwareUpdateRecord() { - string record = ""; + string record = ""; - if (firmwareUpdateFile != NULL) - { - char buffer[MAXFIRMWAREUPDATERECORDSIZE]; + if (firmwareUpdateFile != NULL) { + char buffer[MAXFIRMWAREUPDATERECORDSIZE]; #if VN_HAVE_SECURE_CRT - int result = fscanf_s(firmwareUpdateFile, "%s\n", buffer, MAXFIRMWAREUPDATERECORDSIZE); + int result = fscanf_s(firmwareUpdateFile, "%s\n", buffer, MAXFIRMWAREUPDATERECORDSIZE); #else - int result = fscanf(firmwareUpdateFile, "%s\n", buffer); + int result = fscanf(firmwareUpdateFile, "%s\n", buffer); #endif - if (result != EOF) - { - record = buffer; - } - } + if (result != EOF) { + record = buffer; + } + } - return record; + return record; } void VnSensor::openFirmwareUpdateFile(std::string filename) { #if VN_HAVE_SECURE_CRT - errno_t error = fopen_s(&firmwareUpdateFile, filename.c_str(), "r"); + errno_t error = fopen_s(&firmwareUpdateFile, filename.c_str(), "r"); #else - firmwareUpdateFile = fopen(filename.c_str(), "r"); + firmwareUpdateFile = fopen(filename.c_str(), "r"); #endif - if (firmwareUpdateFile == NULL) { - throw unknown_error(); - } + if (firmwareUpdateFile == NULL) { + throw unknown_error(); + } } void VnSensor::closeFirmwareUpdateFile() { - if (firmwareUpdateFile != NULL) - { - fclose(firmwareUpdateFile); - firmwareUpdateFile = NULL; - } + if (firmwareUpdateFile != NULL) { + fclose(firmwareUpdateFile); + firmwareUpdateFile = NULL; + } } - #if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 -void VnSensor::stopRequest() -{ - _pi->port->stopThread(); -} +void VnSensor::stopRequest() { _pi->port->stopThread(); } -bool VnSensor::threadStopped() -{ - return _pi->port->threadStopped(); -} +bool VnSensor::threadStopped() { return _pi->port->threadStopped(); } -void VnSensor::unregisterListners() -{ - _pi->port->unregisterDataReceivedHandler(); -} +void VnSensor::unregisterListners() { _pi->port->unregisterDataReceivedHandler(); } void VnSensor::shutdownRequest() { - if (_pi->DidWeOpenSimplePort) - { - _pi->port->close(); - } - - _pi->DidWeOpenSimplePort = false; + if (_pi->DidWeOpenSimplePort) { + _pi->port->close(); + } - if (_pi->SimplePortIsOurs) - { - delete _pi->port; + _pi->DidWeOpenSimplePort = false; - _pi->port = NULL; - } + if (_pi->SimplePortIsOurs) { + delete _pi->port; + _pi->port = NULL; + } } -void VnSensor::goRequest() -{ - _pi->port->resumeThread(); -} +void VnSensor::goRequest() { _pi->port->resumeThread(); } #endif -} -} +} // namespace sensors +} // namespace vn diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/serialport.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/serialport.cpp index 3de99a45..285bbbe5 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/serialport.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/serialport.cpp @@ -1,400 +1,353 @@ #include "vn/serialport.h" #if _WIN32 - #include - #include - #include - #include - #if _UNICODE - #else - #include - #endif +#include +#include +#include +#include +#if _UNICODE +#else +#include +#endif #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - #include - #include - #include - #include - #include - #include - #include - #include - #include +#include +#include +#include +#include +#include +#include +#include + +#include +#include #else - #error "Unknown System" +#error "Unknown System" #endif #if __linux__ - #include +#include #elif __APPLE__ - #include +#include #endif -#include #include +#include -#include "vn/thread.h" +#include "vn/compiler.h" #include "vn/criticalsection.h" -#include "vn/exceptions.h" #include "vn/event.h" -#include "vn/compiler.h" +#include "vn/exceptions.h" +#include "vn/thread.h" #if PYTHON - #include "util.h" +#include "util.h" #endif using namespace std; -namespace vn { -namespace xplat { +namespace vn +{ +namespace xplat +{ struct SerialPort::Impl { + // Constants ////////////////////////////////////////////////////////////// - // Constants ////////////////////////////////////////////////////////////// - - static const size_t NumberOfBytesToPurgeOnOpeningSerialPort = 100; - - static const uint8_t WaitTimeForSerialPortReadsInMs = 100; - - // Members //////////////////////////////////////////////////////////////// - - #if _WIN32 - HANDLE SerialPortHandle; - size_t NumberOfReceiveDataDroppedSections; - // Windows appears to need single-thread access to read/write API functions. - CriticalSection ReadWriteCS; - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - int SerialPortHandle; - #else - #error "Unknown System" - #endif - - Thread *pThreadForHandlingReceivedDataInternally; - - // The name of the serial port. - string PortName; - - // The serial port's baudrate. - uint32_t Baudrate; - - // Indicates if the serial port is open. - bool IsOpen; - - // Critical section for registering, unregistering, and notifying observers - // of events. - CriticalSection ObserversCriticalSection; - - DataReceivedHandler _dataReceivedHandler; - void* _dataReceivedUserData; - - Thread *pSerialPortEventsThread; - - bool ContinueHandlingSerialPortEvents; - bool ChangingBaudrate; - - bool PurgeFirstDataBytesWhenSerialPortIsFirstOpened; - - bool ThreadIsRunning; - - SerialPort* BackReference; - - StopBits stopBits; - - Event WaitForBaudrateChange; - Event NotificationsThreadStopped; - - #if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 - bool ExternalStopRequest; - bool ThreadStopped; - #endif - - explicit Impl(SerialPort* backReference) : - #if _WIN32 - NumberOfReceiveDataDroppedSections(0), - SerialPortHandle(NULL), - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - SerialPortHandle(0), - #else - #error "Unknown System" - #endif - pThreadForHandlingReceivedDataInternally(NULL), - Baudrate(0), - IsOpen(false), - _dataReceivedHandler(NULL), - _dataReceivedUserData(NULL), - pSerialPortEventsThread(NULL), - ContinueHandlingSerialPortEvents(false), - ChangingBaudrate(false), - PurgeFirstDataBytesWhenSerialPortIsFirstOpened(true), - #if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 - ExternalStopRequest(false), - ThreadStopped(false), - #endif - ThreadIsRunning(false), - BackReference(backReference), - stopBits(ONE_STOP_BIT) - { } + static const size_t NumberOfBytesToPurgeOnOpeningSerialPort = 100; - ~Impl() - { - } + static const uint8_t WaitTimeForSerialPortReadsInMs = 100; - static void HandleSerialPortNotifications(void* data) - { - static_cast(data)->HandleSerialPortNotifications(); - } + // Members //////////////////////////////////////////////////////////////// - void close(bool checkAndToggleIsOpenFlag = true) - { - if (checkAndToggleIsOpenFlag) - ensureOpened(); +#if _WIN32 + HANDLE SerialPortHandle; + size_t NumberOfReceiveDataDroppedSections; + // Windows appears to need single-thread access to read/write API functions. + CriticalSection ReadWriteCS; +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + int SerialPortHandle; +#else +#error "Unknown System" +#endif - StopSerialPortNotificationsThread(); - - #if _WIN32 - - if (!CloseHandle(SerialPortHandle)) - throw unknown_error(); - - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - - if (::close(SerialPortHandle) == -1) - throw unknown_error(); - - #else - #error "Unknown System" - #endif - - if (checkAndToggleIsOpenFlag) - IsOpen = false; - } + Thread * pThreadForHandlingReceivedDataInternally; - void closeAfterUsbCableUnplugged() - { - #if _WIN32 - - if (!CloseHandle(SerialPortHandle)) - throw unknown_error(); - - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - - if (::close(SerialPortHandle) == -1) - throw unknown_error(); - - #else - #error "Unknown System" - #endif - - IsOpen = false; - } - - void HandleSerialPortNotifications() - { - bool userUnpluggedUsbCable = false; - - #if _WIN32 - - OVERLAPPED overlapped; - - memset(&overlapped, 0, sizeof(OVERLAPPED)); - - overlapped.hEvent = CreateEvent( - NULL, - false, - false, - NULL); - - SetCommMask( - SerialPortHandle, - EV_RXCHAR | EV_ERR | EV_RX80FULL); - - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - - fd_set readfs; - int error; - timeval readWaitTime; - - #else - - #error "Unknown System" - - #endif - - IgnoreError: + // The name of the serial port. + string PortName; - try - { - ThreadIsRunning = true; - - while (ContinueHandlingSerialPortEvents) - { - #if _WIN32 - - #if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 - - if (ExternalStopRequest) - { - ThreadStopped = true; - while (ExternalStopRequest) - { - Sleep(1); - } - ThreadStopped = false; - } - - #endif - - DWORD mask = 0; - DWORD temp = 0; - - BOOL result = WaitCommEvent( - SerialPortHandle, - &mask, - &overlapped); - - if (result) - { - OnDataReceived(); - - continue; - } - - if (GetLastError() != ERROR_IO_PENDING) - // Something unexpected happened. - break; - - KeepWaiting: - - // We need to wait for the event to occur. - DWORD waitResult = WaitForSingleObject( - overlapped.hEvent, - WaitTimeForSerialPortReadsInMs); - - if (!ContinueHandlingSerialPortEvents) - break; - - if (waitResult == WAIT_TIMEOUT) - goto KeepWaiting; - - if (waitResult != WAIT_OBJECT_0) - // Something unexpected happened. - break; - - if (!GetOverlappedResult( - SerialPortHandle, - &overlapped, - &temp, - TRUE)) - { - if (GetLastError() == ERROR_OPERATION_ABORTED) - { - // Possibly the user unplugged the UART-to-USB cable. - ContinueHandlingSerialPortEvents = false; - userUnpluggedUsbCable = true; - } - - // Something unexpected happened. - break; - } - - if (mask & EV_RXCHAR) - { - OnDataReceived(); - - continue; - } - - if (mask & EV_RX80FULL) - { - // We assume the RX buffer was overrun. - NumberOfReceiveDataDroppedSections++; - - continue; - } - - if (mask & EV_ERR) - { - DWORD spErrors; - COMSTAT comStat; - - if (!ClearCommError( - SerialPortHandle, - &spErrors, - &comStat)) - { - // Something unexpected happened. - break; - } - - if ((spErrors & CE_OVERRUN) || (spErrors & CE_RXOVER)) - { - // The serial buffer RX buffer was overrun. - NumberOfReceiveDataDroppedSections++; - } - - continue; - } - - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - - FD_ZERO(&readfs); - FD_SET(SerialPortHandle, &readfs); - - // Select sets the values in readWaitTime. - readWaitTime.tv_sec = 0; - readWaitTime.tv_usec = WaitTimeForSerialPortReadsInMs * 1000; - - error = select( - SerialPortHandle + 1, - &readfs, - NULL, - NULL, - &readWaitTime); - - if (error == -1) - { - #if __CYGWIN__ - - if (errno == EINVAL) - { - // Sometime when running the example getting_started, - // this condition will hit. I assume it is a race - // condition with the operating system (actually this - // problem was noticed running CYGWIN) but appears to - // work when we try it again later. - goto IgnoreError; - } - - #endif - - // Something unexpected happened. - break; - } - - if (!FD_ISSET(SerialPortHandle, &readfs)) - continue; - - OnDataReceived(); - - #else - #error "Unknown System" - #endif - - } - } - catch (...) - { - // Don't want user-code exceptions stopping the thread. - goto IgnoreError; - } + // The serial port's baudrate. + uint32_t Baudrate; + + // Indicates if the serial port is open. + bool IsOpen; + + // Critical section for registering, unregistering, and notifying observers + // of events. + CriticalSection ObserversCriticalSection; + + DataReceivedHandler _dataReceivedHandler; + void * _dataReceivedUserData; + + Thread * pSerialPortEventsThread; + + bool ContinueHandlingSerialPortEvents; + bool ChangingBaudrate; + + bool PurgeFirstDataBytesWhenSerialPortIsFirstOpened; + + bool ThreadIsRunning; + + SerialPort * BackReference; + + StopBits stopBits; + + Event WaitForBaudrateChange; + Event NotificationsThreadStopped; + +#if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 + bool ExternalStopRequest; + bool ThreadStopped; +#endif + + explicit Impl(SerialPort * backReference) + : +#if _WIN32 + NumberOfReceiveDataDroppedSections(0), + SerialPortHandle(NULL), +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + SerialPortHandle(0), +#else +#error "Unknown System" +#endif + pThreadForHandlingReceivedDataInternally(NULL), + Baudrate(0), + IsOpen(false), + _dataReceivedHandler(NULL), + _dataReceivedUserData(NULL), + pSerialPortEventsThread(NULL), + ContinueHandlingSerialPortEvents(false), + ChangingBaudrate(false), + PurgeFirstDataBytesWhenSerialPortIsFirstOpened(true), +#if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 + ExternalStopRequest(false), + ThreadStopped(false), +#endif + ThreadIsRunning(false), + BackReference(backReference), + stopBits(ONE_STOP_BIT) + { + } + + ~Impl() {} + + static void HandleSerialPortNotifications(void * data) + { + static_cast(data)->HandleSerialPortNotifications(); + } + + void close(bool checkAndToggleIsOpenFlag = true) + { + if (checkAndToggleIsOpenFlag) ensureOpened(); + + StopSerialPortNotificationsThread(); + +#if _WIN32 + + if (!CloseHandle(SerialPortHandle)) throw unknown_error(); + +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + if (::close(SerialPortHandle) == -1) throw unknown_error(); + +#else +#error "Unknown System" +#endif + + if (checkAndToggleIsOpenFlag) IsOpen = false; + } + + void closeAfterUsbCableUnplugged() + { +#if _WIN32 + + if (!CloseHandle(SerialPortHandle)) throw unknown_error(); + +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + if (::close(SerialPortHandle) == -1) throw unknown_error(); + +#else +#error "Unknown System" +#endif + + IsOpen = false; + } + + void HandleSerialPortNotifications() + { + bool userUnpluggedUsbCable = false; + +#if _WIN32 + + OVERLAPPED overlapped; - ThreadIsRunning = false; + memset(&overlapped, 0, sizeof(OVERLAPPED)); - if (ContinueHandlingSerialPortEvents) - // An error must have occurred. - throw unknown_error(); + overlapped.hEvent = CreateEvent(NULL, false, false, NULL); + + SetCommMask(SerialPortHandle, EV_RXCHAR | EV_ERR | EV_RX80FULL); + +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + fd_set readfs; + int error; + timeval readWaitTime; + +#else + +#error "Unknown System" + +#endif - /*if (ChangingBaudrate) + IgnoreError: + + try { + ThreadIsRunning = true; + + while (ContinueHandlingSerialPortEvents) { +#if _WIN32 + +#if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 + + if (ExternalStopRequest) { + ThreadStopped = true; + while (ExternalStopRequest) { + Sleep(1); + } + ThreadStopped = false; + } + +#endif + + DWORD mask = 0; + DWORD temp = 0; + + BOOL result = WaitCommEvent(SerialPortHandle, &mask, &overlapped); + + if (result) { + OnDataReceived(); + + continue; + } + + if (GetLastError() != ERROR_IO_PENDING) + // Something unexpected happened. + break; + + KeepWaiting: + + // We need to wait for the event to occur. + DWORD waitResult = WaitForSingleObject(overlapped.hEvent, WaitTimeForSerialPortReadsInMs); + + if (!ContinueHandlingSerialPortEvents) break; + + if (waitResult == WAIT_TIMEOUT) goto KeepWaiting; + + if (waitResult != WAIT_OBJECT_0) + // Something unexpected happened. + break; + + if (!GetOverlappedResult(SerialPortHandle, &overlapped, &temp, TRUE)) { + if (GetLastError() == ERROR_OPERATION_ABORTED) { + // Possibly the user unplugged the UART-to-USB cable. + ContinueHandlingSerialPortEvents = false; + userUnpluggedUsbCable = true; + } + + // Something unexpected happened. + break; + } + + if (mask & EV_RXCHAR) { + OnDataReceived(); + + continue; + } + + if (mask & EV_RX80FULL) { + // We assume the RX buffer was overrun. + NumberOfReceiveDataDroppedSections++; + + continue; + } + + if (mask & EV_ERR) { + DWORD spErrors; + COMSTAT comStat; + + if (!ClearCommError(SerialPortHandle, &spErrors, &comStat)) { + // Something unexpected happened. + break; + } + + if ((spErrors & CE_OVERRUN) || (spErrors & CE_RXOVER)) { + // The serial buffer RX buffer was overrun. + NumberOfReceiveDataDroppedSections++; + } + + continue; + } + +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + FD_ZERO(&readfs); + FD_SET(SerialPortHandle, &readfs); + + // Select sets the values in readWaitTime. + readWaitTime.tv_sec = 0; + readWaitTime.tv_usec = WaitTimeForSerialPortReadsInMs * 1000; + + error = select(SerialPortHandle + 1, &readfs, NULL, NULL, &readWaitTime); + + if (error == -1) { +#if __CYGWIN__ + + if (errno == EINVAL) { + // Sometime when running the example getting_started, + // this condition will hit. I assume it is a race + // condition with the operating system (actually this + // problem was noticed running CYGWIN) but appears to + // work when we try it again later. + goto IgnoreError; + } + +#endif + + // Something unexpected happened. + break; + } + + if (!FD_ISSET(SerialPortHandle, &readfs)) continue; + + OnDataReceived(); + +#else +#error "Unknown System" +#endif + } + } catch (...) { + // Don't want user-code exceptions stopping the thread. + goto IgnoreError; + } + + ThreadIsRunning = false; + + if (ContinueHandlingSerialPortEvents) + // An error must have occurred. + throw unknown_error(); + + /*if (ChangingBaudrate) { NotificationsThreadStopped.signal(); WaitForBaudrateChange.wait(); @@ -402,388 +355,332 @@ struct SerialPort::Impl goto IgnoreError; }*/ - #if _WIN32 +#if _WIN32 - if (!userUnpluggedUsbCable) - { - SetCommMask( - SerialPortHandle, - 0); - } + if (!userUnpluggedUsbCable) { + SetCommMask(SerialPortHandle, 0); + } - #endif +#endif - if (userUnpluggedUsbCable) - closeAfterUsbCableUnplugged(); - } + if (userUnpluggedUsbCable) closeAfterUsbCableUnplugged(); + } - void StartSerialPortNotificationsThread() - { - ContinueHandlingSerialPortEvents = true; + void StartSerialPortNotificationsThread() + { + ContinueHandlingSerialPortEvents = true; - pSerialPortEventsThread = Thread::startNew( - HandleSerialPortNotifications, - this); - } + pSerialPortEventsThread = Thread::startNew(HandleSerialPortNotifications, this); + } - void PurgeFirstDataBytesFromSerialPort() + void PurgeFirstDataBytesFromSerialPort() - { - char buffer[NumberOfBytesToPurgeOnOpeningSerialPort]; - size_t numOfBytesRead; + { + char buffer[NumberOfBytesToPurgeOnOpeningSerialPort]; + size_t numOfBytesRead; - BackReference->read( - buffer, - NumberOfBytesToPurgeOnOpeningSerialPort, - numOfBytesRead); - } + BackReference->read(buffer, NumberOfBytesToPurgeOnOpeningSerialPort, numOfBytesRead); + } - void StopSerialPortNotificationsThread() - { - ContinueHandlingSerialPortEvents = false; + void StopSerialPortNotificationsThread() + { + ContinueHandlingSerialPortEvents = false; - pSerialPortEventsThread->join(); + pSerialPortEventsThread->join(); - delete pSerialPortEventsThread; - } + delete pSerialPortEventsThread; + } - void OnDataReceived() - { - bool exception_happened = false; - exception rethrow; + void OnDataReceived() + { + bool exception_happened = false; + exception rethrow; - ObserversCriticalSection.enter(); + ObserversCriticalSection.enter(); - // This is a critical section block - // The exception must be handled here or the critical section will never - // unlock - try - { - // Moved this NULL check down here due to the nature of critical sections. - // The handler could easily be changed to NULL while waiting for the lock - if (_dataReceivedHandler != NULL) - { - _dataReceivedHandler(_dataReceivedUserData); - } - } - catch (exception& e) - { - // We still want to throw an exception if it happens - // Set a flag to indicate we need to throw an exception - exception_happened = true; - rethrow = e; - } + // This is a critical section block + // The exception must be handled here or the critical section will never + // unlock + try { + // Moved this NULL check down here due to the nature of critical sections. + // The handler could easily be changed to NULL while waiting for the lock + if (_dataReceivedHandler != NULL) { + _dataReceivedHandler(_dataReceivedUserData); + } + } catch (exception & e) { + // We still want to throw an exception if it happens + // Set a flag to indicate we need to throw an exception + exception_happened = true; + rethrow = e; + } - ObserversCriticalSection.leave(); + ObserversCriticalSection.leave(); - // Rethrow the exception - if (exception_happened) - { - throw rethrow; - } - } - - void ensureOpened() - { - if (!IsOpen) - throw invalid_operation("Port is not opened."); - } - - void ensureClosed() - { - if (IsOpen) - throw invalid_operation("Port is not closed."); - } - - void open(bool checkAndToggleIsOpenFlag = true) - { - if (checkAndToggleIsOpenFlag) - ensureClosed(); - - #if _WIN32 - - DCB config; - COMMTIMEOUTS comTimeOut; - - string fullPortName = "\\\\.\\" + PortName; - - SerialPortHandle = CreateFileA( - fullPortName.c_str(), - GENERIC_READ | GENERIC_WRITE, - 0, - NULL, - OPEN_EXISTING, - FILE_FLAG_OVERLAPPED, - NULL); - - if (SerialPortHandle == INVALID_HANDLE_VALUE) - { - DWORD error = GetLastError(); - - if (error == ERROR_ACCESS_DENIED) - // Port already open, probably. - throw invalid_operation("Port '" + PortName + "' already open."); - - if (error == ERROR_FILE_NOT_FOUND) - { - // Port probably does not exist. - char err[256]; - - #if defined(_MSC_VER) - /* Disable warnings regarding using strcpy_s since this + // Rethrow the exception + if (exception_happened) { + throw rethrow; + } + } + + void ensureOpened() + { + if (!IsOpen) throw invalid_operation("Port is not opened."); + } + + void ensureClosed() + { + if (IsOpen) throw invalid_operation("Port is not closed."); + } + + void open(bool checkAndToggleIsOpenFlag = true) + { + if (checkAndToggleIsOpenFlag) ensureClosed(); + +#if _WIN32 + + DCB config; + COMMTIMEOUTS comTimeOut; + + string fullPortName = "\\\\.\\" + PortName; + + SerialPortHandle = CreateFileA( + fullPortName.c_str(), GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, + FILE_FLAG_OVERLAPPED, NULL); + + if (SerialPortHandle == INVALID_HANDLE_VALUE) { + DWORD error = GetLastError(); + + if (error == ERROR_ACCESS_DENIED) + // Port already open, probably. + throw invalid_operation("Port '" + PortName + "' already open."); + + if (error == ERROR_FILE_NOT_FOUND) { + // Port probably does not exist. + char err[256]; + +#if defined(_MSC_VER) + /* Disable warnings regarding using strcpy_s since this * function's signature does not provide us with information * about the length of 'out'. */ - #pragma warning(push) - #pragma warning(disable:4996) - #endif +#pragma warning(push) +#pragma warning(disable : 4996) +#endif - sprintf(err, "Port '%s' not found.", PortName.c_str()); + sprintf(err, "Port '%s' not found.", PortName.c_str()); - #if defined(_MSC_VER) - #pragma warning(pop) - #endif - throw not_found(err); - } +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + throw not_found(err); + } - throw unknown_error(); - } + throw unknown_error(); + } - // Set the state of the COM port. - if (!GetCommState(SerialPortHandle, &config)) - { - DWORD error = GetLastError(); - - if (error != ERROR_OPERATION_ABORTED) - throw unknown_error(); - - // Try clearing this error. - DWORD errors; - if (!ClearCommError(SerialPortHandle, &errors, NULL)) - throw unknown_error(); - - // Retry the operation. - if (!GetCommState(SerialPortHandle, &config)) - throw unknown_error(); - } - - switch (stopBits) - { - case ONE_STOP_BIT: - config.StopBits = ONESTOPBIT; - break; - - case TWO_STOP_BITS: - config.StopBits = TWOSTOPBITS; - break; - - default: - throw not_implemented(); - } - - config.BaudRate = Baudrate; - config.Parity = NOPARITY; - config.ByteSize = 8; - config.fAbortOnError = 0; - - if (!SetCommState(SerialPortHandle, &config)) - { - DWORD error = GetLastError(); + // Set the state of the COM port. + if (!GetCommState(SerialPortHandle, &config)) { + DWORD error = GetLastError(); - if (error == ERROR_INVALID_PARAMETER) - { - if (!CloseHandle(SerialPortHandle)) - throw unknown_error(); + if (error != ERROR_OPERATION_ABORTED) throw unknown_error(); - throw invalid_argument("Unsupported baudrate."); - } + // Try clearing this error. + DWORD errors; + if (!ClearCommError(SerialPortHandle, &errors, NULL)) throw unknown_error(); - if (error != ERROR_OPERATION_ABORTED) - throw unknown_error(); + // Retry the operation. + if (!GetCommState(SerialPortHandle, &config)) throw unknown_error(); + } - // Try clearing this error. - DWORD errors; - if (!ClearCommError(SerialPortHandle, &errors, NULL)) - throw unknown_error(); + switch (stopBits) { + case ONE_STOP_BIT: + config.StopBits = ONESTOPBIT; + break; - // Retry the operation. - if (!SetCommState(SerialPortHandle, &config)) - throw unknown_error(); - } + case TWO_STOP_BITS: + config.StopBits = TWOSTOPBITS; + break; - comTimeOut.ReadIntervalTimeout = 0; - comTimeOut.ReadTotalTimeoutMultiplier = 0; - comTimeOut.ReadTotalTimeoutConstant = 1; - comTimeOut.WriteTotalTimeoutMultiplier = 3; - comTimeOut.WriteTotalTimeoutConstant = 2; + default: + throw not_implemented(); + } - if (!SetCommTimeouts(SerialPortHandle, &comTimeOut)) - { - DWORD error = GetLastError(); + config.BaudRate = Baudrate; + config.Parity = NOPARITY; + config.ByteSize = 8; + config.fAbortOnError = 0; - if (error != ERROR_OPERATION_ABORTED) - throw unknown_error(); + if (!SetCommState(SerialPortHandle, &config)) { + DWORD error = GetLastError(); - // Try clearing this error. - DWORD errors; - if (!ClearCommError(SerialPortHandle, &errors, NULL)) - throw unknown_error(); + if (error == ERROR_INVALID_PARAMETER) { + if (!CloseHandle(SerialPortHandle)) throw unknown_error(); - // Retry the operation. - if (!SetCommTimeouts(SerialPortHandle, &comTimeOut)) - throw unknown_error(); - } + throw invalid_argument("Unsupported baudrate."); + } - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + if (error != ERROR_OPERATION_ABORTED) throw unknown_error(); - int portFd = -1; + // Try clearing this error. + DWORD errors; + if (!ClearCommError(SerialPortHandle, &errors, NULL)) throw unknown_error(); - portFd = ::open( - PortName.c_str(), - #if __linux__ || __CYGWIN__ || __QNXNTO__ - O_RDWR | O_NOCTTY); - #elif __APPLE__ - O_RDWR | O_NOCTTY | O_NONBLOCK); - #else - #error "Unknown System" - #endif + // Retry the operation. + if (!SetCommState(SerialPortHandle, &config)) throw unknown_error(); + } - if (portFd == -1) - { - switch (errno) - { - case EACCES: - throw permission_denied(PortName); - case ENXIO: - case ENOTDIR: - case ENOENT: - throw not_found(PortName); - default: - throw unknown_error(); - } - } - - termios portSettings; - - memset( - &portSettings, - 0, - sizeof(termios)); - - tcflag_t baudrateFlag; - - switch (Baudrate) - { - case 9600: - baudrateFlag = B9600; - break; - case 19200: - baudrateFlag = B19200; - break; - case 38400: - baudrateFlag = B38400; - break; - case 57600: - baudrateFlag = B57600; - break; - case 115200: - baudrateFlag = B115200; - break; - - // QNX does not have higher baudrates defined. - #if !defined(__QNXNTO__) - - case 230400: - baudrateFlag = B230400; - break; - - // Not available on Mac OS X??? - #if !defined(__APPLE__) - - case 460800: - baudrateFlag = B460800; - break; - case 921600: - baudrateFlag = B921600; - break; - - #endif - - #endif - - default: - throw unknown_error(); - } - - // Set baudrate, 8n1, no modem control, and enable receiving characters. - #if __linux__ || __CYGWIN__ || __QNXNTO__ - portSettings.c_cflag = baudrateFlag; - #elif __APPLE__ - cfsetspeed(&portSettings, baudrateFlag); - #endif - portSettings.c_cflag |= 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 characters are received. - - // Clear the serial port buffers. - if (tcflush(portFd, TCIFLUSH) != 0) - throw unknown_error(); - - if (tcsetattr(portFd, TCSANOW, &portSettings) != 0) - throw unknown_error(); - - SerialPortHandle = portFd; - - #else - #error "Unknown System" - #endif - - if (checkAndToggleIsOpenFlag) - IsOpen = true; - - if (PurgeFirstDataBytesWhenSerialPortIsFirstOpened) - PurgeFirstDataBytesFromSerialPort(); - - StartSerialPortNotificationsThread(); - } + comTimeOut.ReadIntervalTimeout = 0; + comTimeOut.ReadTotalTimeoutMultiplier = 0; + comTimeOut.ReadTotalTimeoutConstant = 1; + comTimeOut.WriteTotalTimeoutMultiplier = 3; + comTimeOut.WriteTotalTimeoutConstant = 2; + + if (!SetCommTimeouts(SerialPortHandle, &comTimeOut)) { + DWORD error = GetLastError(); + + if (error != ERROR_OPERATION_ABORTED) throw unknown_error(); + + // Try clearing this error. + DWORD errors; + if (!ClearCommError(SerialPortHandle, &errors, NULL)) throw unknown_error(); + + // Retry the operation. + if (!SetCommTimeouts(SerialPortHandle, &comTimeOut)) throw unknown_error(); + } + +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + int portFd = -1; + + portFd = ::open( + PortName.c_str(), +#if __linux__ || __CYGWIN__ || __QNXNTO__ + O_RDWR | O_NOCTTY); +#elif __APPLE__ + O_RDWR | O_NOCTTY | O_NONBLOCK); +#else +#error "Unknown System" +#endif + + if (portFd == -1) { + switch (errno) { + case EACCES: + throw permission_denied(PortName); + case ENXIO: + case ENOTDIR: + case ENOENT: + throw not_found(PortName); + default: + throw unknown_error(); + } + } + + termios portSettings; + + memset(&portSettings, 0, sizeof(termios)); + + tcflag_t baudrateFlag; + + switch (Baudrate) { + case 9600: + baudrateFlag = B9600; + break; + case 19200: + baudrateFlag = B19200; + break; + case 38400: + baudrateFlag = B38400; + break; + case 57600: + baudrateFlag = B57600; + break; + case 115200: + baudrateFlag = B115200; + break; + +// QNX does not have higher baudrates defined. +#if !defined(__QNXNTO__) + + case 230400: + baudrateFlag = B230400; + break; + +// Not available on Mac OS X??? +#if !defined(__APPLE__) + + case 460800: + baudrateFlag = B460800; + break; + case 921600: + baudrateFlag = B921600; + break; + +#endif + +#endif + + default: + throw unknown_error(); + } + +// Set baudrate, 8n1, no modem control, and enable receiving characters. +#if __linux__ || __CYGWIN__ || __QNXNTO__ + portSettings.c_cflag = baudrateFlag; +#elif __APPLE__ + cfsetspeed(&portSettings, baudrateFlag); +#endif + portSettings.c_cflag |= 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 characters are received. + + // Clear the serial port buffers. + if (tcflush(portFd, TCIFLUSH) != 0) throw unknown_error(); + + if (tcsetattr(portFd, TCSANOW, &portSettings) != 0) throw unknown_error(); + + SerialPortHandle = portFd; + +#else +#error "Unknown System" +#endif + + if (checkAndToggleIsOpenFlag) IsOpen = true; + + if (PurgeFirstDataBytesWhenSerialPortIsFirstOpened) PurgeFirstDataBytesFromSerialPort(); + + StartSerialPortNotificationsThread(); + } }; #if defined(_MSC_VER) - #pragma warning(push) - #pragma warning(disable:4355) +#pragma warning(push) +#pragma warning(disable : 4355) #endif -SerialPort::SerialPort( - const string &portName, - uint32_t baudrate) : - _pi(new Impl(this)) +SerialPort::SerialPort(const string & portName, uint32_t baudrate) : _pi(new Impl(this)) { - _pi->PortName = portName; - _pi->Baudrate = baudrate; + _pi->PortName = portName; + _pi->Baudrate = baudrate; } -#if defined (_MSC_VER) - #pragma warning(pop) +#if defined(_MSC_VER) +#pragma warning(pop) #endif SerialPort::~SerialPort() { - if (_pi->IsOpen) - { - try - { - close(); - } - catch (...) - { - // Something happened but don't want to throw out of the - // destructor. - } - } + if (_pi->IsOpen) { + try { + close(); + } catch (...) { + // Something happened but don't want to throw out of the + // destructor. + } + } - delete _pi; + delete _pi; } #if _WIN32 @@ -794,662 +691,499 @@ SerialPort::~SerialPort() /// \return true if this is an FTDI USB serial port; otherwise false. bool SerialPort_isFtdiUsbSerialPort(string portName) { - HDEVINFO deviceInfoSet = SetupDiGetClassDevs( - &GUID_DEVCLASS_PORTS, - NULL, - NULL, - DIGCF_PRESENT); - - if (deviceInfoSet == INVALID_HANDLE_VALUE) - throw unknown_error(); - - SP_DEVINFO_DATA deviceData; - ZeroMemory(&deviceData, sizeof(SP_DEVINFO_DATA)); - deviceData.cbSize = sizeof(SP_DEVINFO_DATA); - DWORD curDevId = 0; - - TCHAR portStrToFind[10]; - TCHAR cPortStr[10]; - - copy(portName.begin(), portName.end(), cPortStr); - cPortStr[portName.size()] = '\0'; - - #if VN_HAVE_SECURE_CRT - _stprintf_s(portStrToFind, sizeof(portStrToFind), TEXT("(%s)"), cPortStr); - #else - _stprintf(portStrToFind, TEXT("(%s)"), cPortStr); - #endif - - bool isFtdiDevice = false; - - while (SetupDiEnumDeviceInfo( - deviceInfoSet, - curDevId, - &deviceData)) - { - curDevId++; - TCHAR friendlyName[0x100]; - - if (!SetupDiGetDeviceRegistryProperty( - deviceInfoSet, - &deviceData, - SPDRP_FRIENDLYNAME, - NULL, - (PBYTE) friendlyName, - sizeof(friendlyName), - NULL)) - { - SetupDiDestroyDeviceInfoList(deviceInfoSet); - - throw unknown_error(); - } - - // See if this device is our COM port. - // TODO: There must be a better way to check the associated COM port number. - if (_tcsstr(friendlyName, portStrToFind) == NULL) - // Not the port we are looking for. - continue; - - // First see if this is an FTDI device. - TCHAR mfgName[0x100]; - if (!SetupDiGetDeviceRegistryProperty( - deviceInfoSet, - &deviceData, - SPDRP_MFG, - NULL, - (PBYTE) mfgName, - sizeof(mfgName), - NULL)) - { - SetupDiDestroyDeviceInfoList(deviceInfoSet); + HDEVINFO deviceInfoSet = SetupDiGetClassDevs(&GUID_DEVCLASS_PORTS, NULL, NULL, DIGCF_PRESENT); + + if (deviceInfoSet == INVALID_HANDLE_VALUE) throw unknown_error(); + + SP_DEVINFO_DATA deviceData; + ZeroMemory(&deviceData, sizeof(SP_DEVINFO_DATA)); + deviceData.cbSize = sizeof(SP_DEVINFO_DATA); + DWORD curDevId = 0; - throw unknown_error(); - } + TCHAR portStrToFind[10]; + TCHAR cPortStr[10]; - // TODO: Possibly better way to check if this is an FTDI USB serial port. - isFtdiDevice = _tcscmp(mfgName, TEXT("FTDI")) == 0; + copy(portName.begin(), portName.end(), cPortStr); + cPortStr[portName.size()] = '\0'; - break; - } +#if VN_HAVE_SECURE_CRT + _stprintf_s(portStrToFind, sizeof(portStrToFind), TEXT("(%s)"), cPortStr); +#else + _stprintf(portStrToFind, TEXT("(%s)"), cPortStr); +#endif - SetupDiDestroyDeviceInfoList(deviceInfoSet); + bool isFtdiDevice = false; + + while (SetupDiEnumDeviceInfo(deviceInfoSet, curDevId, &deviceData)) { + curDevId++; + TCHAR friendlyName[0x100]; + + if (!SetupDiGetDeviceRegistryProperty( + deviceInfoSet, &deviceData, SPDRP_FRIENDLYNAME, NULL, (PBYTE)friendlyName, + sizeof(friendlyName), NULL)) { + SetupDiDestroyDeviceInfoList(deviceInfoSet); + + throw unknown_error(); + } - return isFtdiDevice; + // See if this device is our COM port. + // TODO: There must be a better way to check the associated COM port number. + if (_tcsstr(friendlyName, portStrToFind) == NULL) + // Not the port we are looking for. + continue; + + // First see if this is an FTDI device. + TCHAR mfgName[0x100]; + if (!SetupDiGetDeviceRegistryProperty( + deviceInfoSet, &deviceData, SPDRP_MFG, NULL, (PBYTE)mfgName, sizeof(mfgName), NULL)) { + SetupDiDestroyDeviceInfoList(deviceInfoSet); + + throw unknown_error(); + } + + // TODO: Possibly better way to check if this is an FTDI USB serial port. + isFtdiDevice = _tcscmp(mfgName, TEXT("FTDI")) == 0; + + break; + } + + SetupDiDestroyDeviceInfoList(deviceInfoSet); + + return isFtdiDevice; } HKEY SerialPort_getRegistryKeyForActiveFtdiPort(string portName, bool isReadOnly) { - HDEVINFO deviceInfoSet = SetupDiGetClassDevs( - &GUID_DEVCLASS_PORTS, - NULL, - NULL, - DIGCF_PRESENT); - - if (deviceInfoSet == INVALID_HANDLE_VALUE) - throw unknown_error(); - - SP_DEVINFO_DATA deviceData; - ZeroMemory(&deviceData, sizeof(SP_DEVINFO_DATA)); - deviceData.cbSize = sizeof(SP_DEVINFO_DATA); - DWORD curDevId = 0; - - TCHAR portStrToFind[10]; - TCHAR cPortStr[10]; - - copy(portName.begin(), portName.end(), cPortStr); - cPortStr[portName.size()] = '\0'; - - #if VN_HAVE_SECURE_CRT - _stprintf_s(portStrToFind, sizeof(portStrToFind), TEXT("(%s)"), cPortStr); - #else - _stprintf(portStrToFind, TEXT("(%s)"), cPortStr); - #endif - - TCHAR deviceInstanceId[0x100]; - - while (SetupDiEnumDeviceInfo( - deviceInfoSet, - curDevId, - &deviceData)) - { - curDevId++; - TCHAR friendlyName[0x100]; - - if (!SetupDiGetDeviceRegistryProperty( - deviceInfoSet, - &deviceData, - SPDRP_FRIENDLYNAME, - NULL, - (PBYTE) friendlyName, - sizeof(friendlyName), - NULL)) - { - SetupDiDestroyDeviceInfoList(deviceInfoSet); - - throw unknown_error(); - } - - // See if this device is our COM port. - // TODO: There must be a better way to check the associated COM port number. - if (_tcsstr(friendlyName, portStrToFind) == NULL) - // Not the port we are looking for. - continue; - - // First see if this is an FTDI device. - TCHAR mfgName[0x100]; - if (!SetupDiGetDeviceRegistryProperty( - deviceInfoSet, - &deviceData, - SPDRP_MFG, - NULL, - (PBYTE) mfgName, - sizeof(mfgName), - NULL)) - { - SetupDiDestroyDeviceInfoList(deviceInfoSet); + HDEVINFO deviceInfoSet = SetupDiGetClassDevs(&GUID_DEVCLASS_PORTS, NULL, NULL, DIGCF_PRESENT); - throw unknown_error(); - } + if (deviceInfoSet == INVALID_HANDLE_VALUE) throw unknown_error(); - if (_tcscmp(mfgName, TEXT("FTDI")) != 0) - { - // This COM port must not be and FTDI. - SetupDiDestroyDeviceInfoList(deviceInfoSet); - - throw invalid_operation(); - } - - // Found our port. Get the Device Instance ID/Name for later when we - // look in the registry. - if (!SetupDiGetDeviceInstanceId( - deviceInfoSet, - &deviceData, - deviceInstanceId, - sizeof(deviceInstanceId), - NULL)) - { - SetupDiDestroyDeviceInfoList(deviceInfoSet); + SP_DEVINFO_DATA deviceData; + ZeroMemory(&deviceData, sizeof(SP_DEVINFO_DATA)); + deviceData.cbSize = sizeof(SP_DEVINFO_DATA); + DWORD curDevId = 0; + + TCHAR portStrToFind[10]; + TCHAR cPortStr[10]; + + copy(portName.begin(), portName.end(), cPortStr); + cPortStr[portName.size()] = '\0'; + +#if VN_HAVE_SECURE_CRT + _stprintf_s(portStrToFind, sizeof(portStrToFind), TEXT("(%s)"), cPortStr); +#else + _stprintf(portStrToFind, TEXT("(%s)"), cPortStr); +#endif + + TCHAR deviceInstanceId[0x100]; + + while (SetupDiEnumDeviceInfo(deviceInfoSet, curDevId, &deviceData)) { + curDevId++; + TCHAR friendlyName[0x100]; - throw unknown_error(); - } + if (!SetupDiGetDeviceRegistryProperty( + deviceInfoSet, &deviceData, SPDRP_FRIENDLYNAME, NULL, (PBYTE)friendlyName, + sizeof(friendlyName), NULL)) { + SetupDiDestroyDeviceInfoList(deviceInfoSet); - break; - } + throw unknown_error(); + } + + // See if this device is our COM port. + // TODO: There must be a better way to check the associated COM port number. + if (_tcsstr(friendlyName, portStrToFind) == NULL) + // Not the port we are looking for. + continue; + + // First see if this is an FTDI device. + TCHAR mfgName[0x100]; + if (!SetupDiGetDeviceRegistryProperty( + deviceInfoSet, &deviceData, SPDRP_MFG, NULL, (PBYTE)mfgName, sizeof(mfgName), NULL)) { + SetupDiDestroyDeviceInfoList(deviceInfoSet); + + throw unknown_error(); + } - SetupDiDestroyDeviceInfoList(deviceInfoSet); + if (_tcscmp(mfgName, TEXT("FTDI")) != 0) { + // This COM port must not be and FTDI. + SetupDiDestroyDeviceInfoList(deviceInfoSet); - // Now look in the registry for the FTDI entry. - HKEY ftdiKey; - TCHAR ftdiKeyPath[0x100]; + throw invalid_operation(); + } + + // Found our port. Get the Device Instance ID/Name for later when we + // look in the registry. + if (!SetupDiGetDeviceInstanceId( + deviceInfoSet, &deviceData, deviceInstanceId, sizeof(deviceInstanceId), NULL)) { + SetupDiDestroyDeviceInfoList(deviceInfoSet); - #if VN_HAVE_SECURE_CRT - _stprintf_s(ftdiKeyPath, sizeof(ftdiKeyPath), TEXT("SYSTEM\\CurrentControlSet\\Enum\\%s\\Device Parameters"), deviceInstanceId); - #else - _stprintf(ftdiKeyPath, TEXT("SYSTEM\\CurrentControlSet\\Enum\\%s\\Device Parameters"), deviceInstanceId); - #endif + throw unknown_error(); + } - REGSAM accessType = isReadOnly ? KEY_READ : KEY_READ | KEY_SET_VALUE; + break; + } - DWORD result = RegOpenKeyEx( - HKEY_LOCAL_MACHINE, - ftdiKeyPath, - 0, - accessType, - &ftdiKey); + SetupDiDestroyDeviceInfoList(deviceInfoSet); - if (result == ERROR_ACCESS_DENIED) - throw permission_denied(); + // Now look in the registry for the FTDI entry. + HKEY ftdiKey; + TCHAR ftdiKeyPath[0x100]; - if (result != ERROR_SUCCESS) - throw unknown_error(); +#if VN_HAVE_SECURE_CRT + _stprintf_s( + ftdiKeyPath, sizeof(ftdiKeyPath), + TEXT("SYSTEM\\CurrentControlSet\\Enum\\%s\\Device Parameters"), deviceInstanceId); +#else + _stprintf( + ftdiKeyPath, TEXT("SYSTEM\\CurrentControlSet\\Enum\\%s\\Device Parameters"), deviceInstanceId); +#endif - return ftdiKey; + REGSAM accessType = isReadOnly ? KEY_READ : KEY_READ | KEY_SET_VALUE; + + DWORD result = RegOpenKeyEx(HKEY_LOCAL_MACHINE, ftdiKeyPath, 0, accessType, &ftdiKey); + + if (result == ERROR_ACCESS_DENIED) throw permission_denied(); + + if (result != ERROR_SUCCESS) throw unknown_error(); + + return ftdiKey; } #endif bool SerialPort::determineIfPortIsOptimized(string portName) { - #if !_WIN32 +#if !_WIN32 - // Don't know of any optimizations that need to be done for non-Windows systems. - return true; + // Don't know of any optimizations that need to be done for non-Windows systems. + return true; - #else +#else - // We used to just search the the FTDI devices listed in the registry and - // locate the first entry that matched the requested portName. However, it - // is possible for multiple FTDI device listings to match the provided - // portName, probably from devices that are currently disconnected with the - // machine. The new technique first look through the PnP devices of the - // machine to first find which devices are active. + // We used to just search the the FTDI devices listed in the registry and + // locate the first entry that matched the requested portName. However, it + // is possible for multiple FTDI device listings to match the provided + // portName, probably from devices that are currently disconnected with the + // machine. The new technique first look through the PnP devices of the + // machine to first find which devices are active. - if (!SerialPort_isFtdiUsbSerialPort(portName)) - // Only FTDI devices are known to require optimizing. - return true; + if (!SerialPort_isFtdiUsbSerialPort(portName)) + // Only FTDI devices are known to require optimizing. + return true; - HKEY ftdiKey = SerialPort_getRegistryKeyForActiveFtdiPort(portName, true); + HKEY ftdiKey = SerialPort_getRegistryKeyForActiveFtdiPort(portName, true); - // Now see if the latency is set to 1. - DWORD latencyTimerValue; - DWORD latencyTimerValueSize = sizeof(latencyTimerValue); + // Now see if the latency is set to 1. + DWORD latencyTimerValue; + DWORD latencyTimerValueSize = sizeof(latencyTimerValue); - if (RegQueryValueEx( - ftdiKey, - TEXT("LatencyTimer"), - NULL, - NULL, - (LPBYTE) &latencyTimerValue, - &latencyTimerValueSize) != ERROR_SUCCESS) - { - throw unknown_error(); - } + if ( + RegQueryValueEx( + ftdiKey, TEXT("LatencyTimer"), NULL, NULL, (LPBYTE)&latencyTimerValue, + &latencyTimerValueSize) != ERROR_SUCCESS) { + throw unknown_error(); + } - return latencyTimerValue == 1; + return latencyTimerValue == 1; - #endif +#endif } void SerialPort::optimizePort(string portName) { - #if !_WIN32 +#if !_WIN32 - throw not_supported(); + throw not_supported(); - #else +#else - HKEY ftdiKey = SerialPort_getRegistryKeyForActiveFtdiPort(portName, false); + HKEY ftdiKey = SerialPort_getRegistryKeyForActiveFtdiPort(portName, false); - DWORD latencyTimerValue = 1; + DWORD latencyTimerValue = 1; - if (RegSetValueEx( - ftdiKey, - TEXT("LatencyTimer"), - 0, - REG_DWORD, - (PBYTE) &latencyTimerValue, - sizeof(DWORD)) != ERROR_SUCCESS) - { - throw unknown_error(); - } + if ( + RegSetValueEx( + ftdiKey, TEXT("LatencyTimer"), 0, REG_DWORD, (PBYTE)&latencyTimerValue, sizeof(DWORD)) != + ERROR_SUCCESS) { + throw unknown_error(); + } - #endif +#endif } std::vector SerialPort::getPortNames() { - std::vector comPorts; - - #if _WIN32 - DWORD numOfSubkeys = 0; - DWORD numOfValues = 0; - - HKEY serialCommKey; - LONG error; - - error = RegOpenKeyEx( - HKEY_LOCAL_MACHINE, - TEXT("HARDWARE\\DEVICEMAP\\SERIALCOMM"), - 0, - KEY_READ, - &serialCommKey); - - if (ERROR_SUCCESS == error) - { - error = RegQueryInfoKey( - serialCommKey, - NULL, - NULL, - NULL, - &numOfSubkeys, - NULL, - NULL, - &numOfValues, - NULL, - NULL, - NULL, - NULL); - } + std::vector comPorts; - if (ERROR_SUCCESS == error) - { - for (size_t i = 0; i < numOfValues; i++) - { - TCHAR data[0x100]; - TCHAR value[0x100]; - DWORD capacity = 0x100; - DWORD dataSize = sizeof(data); - - error = RegEnumValue( - serialCommKey, - i, - value, - &capacity, - NULL, - NULL, - (LPBYTE) data, - &dataSize); - - if (error != ERROR_SUCCESS) - throw unknown_error(); - - #ifdef UNICODE - - char converted[0x100]; - int convertResult = WideCharToMultiByte( - CP_ACP, - 0, - data, - dataSize, - converted, - sizeof(converted), - NULL, - NULL); - - if (convertResult == 0) - throw unknown_error(); - - comPorts.push_back(string(converted)); - - #else - comPorts.push_back(string(data)); - #endif - } - } +#if _WIN32 + DWORD numOfSubkeys = 0; + DWORD numOfValues = 0; - // A file not found error is acceptable here and should not throw an error. - // This simply indicates that no sensors are attached to the system. - if ((ERROR_SUCCESS != error) && - (ERROR_FILE_NOT_FOUND != error)) - { - throw unknown_error(); - } + HKEY serialCommKey; + LONG error; - #elif __linux__ || __CYGWIN__ || __QNXNTO__ + error = RegOpenKeyEx( + HKEY_LOCAL_MACHINE, TEXT("HARDWARE\\DEVICEMAP\\SERIALCOMM"), 0, KEY_READ, &serialCommKey); - // comPorts; + if (ERROR_SUCCESS == error) { + error = RegQueryInfoKey( + serialCommKey, NULL, NULL, NULL, &numOfSubkeys, NULL, NULL, &numOfValues, NULL, NULL, NULL, + NULL); + } - int portFd = -1; - const size_t MAX_PORTS = 255; - std::string portName; - std::stringstream stream; + if (ERROR_SUCCESS == error) { + for (size_t i = 0; i < numOfValues; i++) { + TCHAR data[0x100]; + TCHAR value[0x100]; + DWORD capacity = 0x100; + DWORD dataSize = sizeof(data); - for(size_t index = 0; index < MAX_PORTS; ++index) - { - stream << "/dev/ttyUSB" << index; - portName = stream.str(); - portFd = ::open(portName.c_str(), - #if __linux__ || __CYGWIN__ || __QNXNTO__ - O_RDWR | O_NOCTTY ); - #else - #error "Unknown System" - #endif + error = RegEnumValue(serialCommKey, i, value, &capacity, NULL, NULL, (LPBYTE)data, &dataSize); - if(portFd != -1) - { - comPorts.push_back(portName); - ::close(portFd); - } + if (error != ERROR_SUCCESS) throw unknown_error(); - portName.clear(); - stream.str(std::string()); - } +#ifdef UNICODE - #elif __APPLE__ + char converted[0x100]; + int convertResult = + WideCharToMultiByte(CP_ACP, 0, data, dataSize, converted, sizeof(converted), NULL, NULL); - DIR *dp = NULL; - struct dirent *dirp; + if (convertResult == 0) throw unknown_error(); - if ((dp = opendir("/dev")) == NULL) - throw unknown_error(); + comPorts.push_back(string(converted)); - while ((dirp = readdir(dp)) != NULL) - { - if (strstr(dirp->d_name, "tty.usbserial") != NULL) - comPorts.push_back(string(dirp->d_name)); - } +#else + comPorts.push_back(string(data)); +#endif + } + } + + // A file not found error is acceptable here and should not throw an error. + // This simply indicates that no sensors are attached to the system. + if ((ERROR_SUCCESS != error) && (ERROR_FILE_NOT_FOUND != error)) { + throw unknown_error(); + } + +#elif __linux__ || __CYGWIN__ || __QNXNTO__ + + // comPorts; + + int portFd = -1; + const size_t MAX_PORTS = 255; + std::string portName; + std::stringstream stream; + + for (size_t index = 0; index < MAX_PORTS; ++index) { + stream << "/dev/ttyUSB" << index; + portName = stream.str(); + portFd = ::open( + portName.c_str(), +#if __linux__ || __CYGWIN__ || __QNXNTO__ + O_RDWR | O_NOCTTY); +#else +#error "Unknown System" +#endif - closedir(dp); + if (portFd != -1) { + comPorts.push_back(portName); + ::close(portFd); + } - #else - #error "Unknown System" - #endif + portName.clear(); + stream.str(std::string()); + } - return comPorts; -} +#elif __APPLE__ -void SerialPort::open() -{ - _pi->open(); -} + DIR * dp = NULL; + struct dirent * dirp; -void SerialPort::close() -{ - _pi->close(); -} + if ((dp = opendir("/dev")) == NULL) throw unknown_error(); -bool SerialPort::isOpen() -{ - return _pi->IsOpen; -} + while ((dirp = readdir(dp)) != NULL) { + if (strstr(dirp->d_name, "tty.usbserial") != NULL) comPorts.push_back(string(dirp->d_name)); + } -SerialPort::StopBits SerialPort::stopBits() -{ - return _pi->stopBits; + closedir(dp); + +#else +#error "Unknown System" +#endif + + return comPorts; } - + +void SerialPort::open() { _pi->open(); } + +void SerialPort::close() { _pi->close(); } + +bool SerialPort::isOpen() { return _pi->IsOpen; } + +SerialPort::StopBits SerialPort::stopBits() { return _pi->stopBits; } + void SerialPort::setStopBits(SerialPort::StopBits stopBits) { - _pi->ensureClosed(); - - _pi->stopBits = stopBits; + _pi->ensureClosed(); + + _pi->stopBits = stopBits; } void SerialPort::write(const char data[], size_t length) { - _pi->ensureOpened(); + _pi->ensureOpened(); - #if _WIN32 +#if _WIN32 - DWORD numOfBytesWritten; - BOOL result; + DWORD numOfBytesWritten; + BOOL result; - OVERLAPPED overlapped; - memset(&overlapped, 0, sizeof(OVERLAPPED)); + OVERLAPPED overlapped; + memset(&overlapped, 0, sizeof(OVERLAPPED)); - _pi->ReadWriteCS.enter(); + _pi->ReadWriteCS.enter(); - result = WriteFile( - _pi->SerialPortHandle, - data, - length, - NULL, - &overlapped); + result = WriteFile(_pi->SerialPortHandle, data, length, NULL, &overlapped); - if (!result && GetLastError() != ERROR_IO_PENDING) - { - _pi->ReadWriteCS.leave(); - throw unknown_error(); - } + if (!result && GetLastError() != ERROR_IO_PENDING) { + _pi->ReadWriteCS.leave(); + throw unknown_error(); + } - result = GetOverlappedResult( - _pi->SerialPortHandle, - &overlapped, - reinterpret_cast(&numOfBytesWritten), - true); + result = GetOverlappedResult( + _pi->SerialPortHandle, &overlapped, reinterpret_cast(&numOfBytesWritten), true); - if (!result) - { - _pi->ReadWriteCS.leave(); - throw unknown_error(); - } + if (!result) { + _pi->ReadWriteCS.leave(); + throw unknown_error(); + } - result = FlushFileBuffers(_pi->SerialPortHandle); + result = FlushFileBuffers(_pi->SerialPortHandle); - _pi->ReadWriteCS.leave(); + _pi->ReadWriteCS.leave(); - if (!result) - throw unknown_error(); + if (!result) throw unknown_error(); - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - ssize_t numOfBytesWritten = ::write( - _pi->SerialPortHandle, - data, - length); + ssize_t numOfBytesWritten = ::write(_pi->SerialPortHandle, data, length); - if (numOfBytesWritten == -1) - throw unknown_error(); + if (numOfBytesWritten == -1) throw unknown_error(); - #else - #error "Unknown System" - #endif +#else +#error "Unknown System" +#endif } -void SerialPort::read(char dataBuffer[], size_t numOfBytesToRead, size_t &numOfBytesActuallyRead) +void SerialPort::read(char dataBuffer[], size_t numOfBytesToRead, size_t & numOfBytesActuallyRead) { - _pi->ensureOpened(); + _pi->ensureOpened(); - #if _WIN32 +#if _WIN32 + + OVERLAPPED overlapped; + memset(&overlapped, 0, sizeof(OVERLAPPED)); - OVERLAPPED overlapped; - memset(&overlapped, 0, sizeof(OVERLAPPED)); + _pi->ReadWriteCS.enter(); - _pi->ReadWriteCS.enter(); + BOOL result = ReadFile(_pi->SerialPortHandle, dataBuffer, numOfBytesToRead, NULL, &overlapped); - BOOL result = ReadFile( - _pi->SerialPortHandle, - dataBuffer, - numOfBytesToRead, - NULL, - &overlapped); - - if (!result && GetLastError() != ERROR_IO_PENDING) - { - _pi->ReadWriteCS.leave(); - throw unknown_error(); - } + if (!result && GetLastError() != ERROR_IO_PENDING) { + _pi->ReadWriteCS.leave(); + throw unknown_error(); + } - result = GetOverlappedResult( - _pi->SerialPortHandle, - &overlapped, - reinterpret_cast(&numOfBytesActuallyRead), - true); + result = GetOverlappedResult( + _pi->SerialPortHandle, &overlapped, reinterpret_cast(&numOfBytesActuallyRead), true); - _pi->ReadWriteCS.leave(); + _pi->ReadWriteCS.leave(); - if (!result) - throw unknown_error(); + if (!result) throw unknown_error(); - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - int result = ::read( - _pi->SerialPortHandle, - dataBuffer, - numOfBytesToRead); + int result = ::read(_pi->SerialPortHandle, dataBuffer, numOfBytesToRead); - if (result == -1) - throw unknown_error(); + if (result == -1) throw unknown_error(); - numOfBytesActuallyRead = static_cast(result); + numOfBytesActuallyRead = static_cast(result); - #else - #error "Unknown System" - #endif +#else +#error "Unknown System" +#endif } -void SerialPort::registerDataReceivedHandler(void* userData, DataReceivedHandler handler) +void SerialPort::registerDataReceivedHandler(void * userData, DataReceivedHandler handler) { - if (_pi->_dataReceivedHandler != NULL) - throw invalid_operation(); + if (_pi->_dataReceivedHandler != NULL) throw invalid_operation(); - _pi->ObserversCriticalSection.enter(); + _pi->ObserversCriticalSection.enter(); - _pi->_dataReceivedHandler = handler; - _pi->_dataReceivedUserData = userData; + _pi->_dataReceivedHandler = handler; + _pi->_dataReceivedUserData = userData; - _pi->ObserversCriticalSection.leave(); + _pi->ObserversCriticalSection.leave(); } void SerialPort::unregisterDataReceivedHandler() { - if (_pi->_dataReceivedHandler == NULL) - throw invalid_operation(); + if (_pi->_dataReceivedHandler == NULL) throw invalid_operation(); - _pi->ObserversCriticalSection.enter(); + _pi->ObserversCriticalSection.enter(); - _pi->_dataReceivedHandler = NULL; - _pi->_dataReceivedUserData = NULL; + _pi->_dataReceivedHandler = NULL; + _pi->_dataReceivedUserData = NULL; - _pi->ObserversCriticalSection.leave(); + _pi->ObserversCriticalSection.leave(); } -uint32_t SerialPort::baudrate() -{ - return _pi->Baudrate; -} +uint32_t SerialPort::baudrate() { return _pi->Baudrate; } -std::string SerialPort::port() -{ - return _pi->PortName; -} +std::string SerialPort::port() { return _pi->PortName; } void SerialPort::changeBaudrate(uint32_t baudrate) { - _pi->ensureOpened(); + _pi->ensureOpened(); - _pi->close(false); + _pi->close(false); - _pi->Baudrate = baudrate; + _pi->Baudrate = baudrate; - _pi->open(false); + _pi->open(false); } size_t SerialPort::NumberOfReceiveDataDroppedSections() { - #if _WIN32 +#if _WIN32 + + return _pi->NumberOfReceiveDataDroppedSections; - return _pi->NumberOfReceiveDataDroppedSections; +#elif __linux__ - #elif __linux__ + serial_icounter_struct serialStatus; - serial_icounter_struct serialStatus; + ioctl(_pi->SerialPortHandle, TIOCGICOUNT, &serialStatus); - ioctl( - _pi->SerialPortHandle, - TIOCGICOUNT, - &serialStatus); + return serialStatus.overrun + serialStatus.buf_overrun; - return serialStatus.overrun + serialStatus.buf_overrun; +#elif __APPLE__ || __CYGWIN__ || __QNXNTO__ - #elif __APPLE__ || __CYGWIN__ || __QNXNTO__ - - // Don't know how to implement this on Mac OS X. - throw not_implemented(); - - #else - #error "Unknown System" - #endif + // Don't know how to implement this on Mac OS X. + throw not_implemented(); + +#else +#error "Unknown System" +#endif } #if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 -void SerialPort::stopThread() -{ - _pi->externalStop(); -} +void SerialPort::stopThread() { _pi->externalStop(); } -bool SerialPort::threadStopped() -{ - return _pi->ThreadStopped; -} +bool SerialPort::threadStopped() { return _pi->ThreadStopped; } -void SerialPort::resumeThread() -{ - _pi->externalStopFinished(); -} +void SerialPort::resumeThread() { _pi->externalStopFinished(); } #endif -} -} +} // namespace xplat +} // namespace vn diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/thread.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/thread.cpp index 58a79996..0572e92f 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/thread.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/thread.cpp @@ -1,182 +1,163 @@ #include "vn/thread.h" + #include "vn/exceptions.h" #if _WIN32 - #include +#include #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - #include - #include +#include +#include #else - #error "Unknown System" +#error "Unknown System" #endif using namespace std; -namespace vn { -namespace xplat { +namespace vn +{ +namespace xplat +{ struct Thread::Impl { - #if _WIN32 - HANDLE ThreadHandle; - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - pthread_t ThreadHandle; - void* Data; - #else - #error "Unknown System" - #endif - - Thread::ThreadStartRoutine StartRoutine; - - Impl() : - #if _WIN32 - ThreadHandle(NULL), - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - ThreadHandle(0), - Data(NULL), - #else - #error "Unknown System" - #endif - StartRoutine(NULL) - { } - - #if __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - - static void* StartRoutineWrapper(void* data) - { - Impl* impl = (Impl*) data; - - impl->StartRoutine(impl->Data); - - return NULL; - } - - #endif +#if _WIN32 + HANDLE ThreadHandle; +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + pthread_t ThreadHandle; + void * Data; +#else +#error "Unknown System" +#endif + + Thread::ThreadStartRoutine StartRoutine; + + Impl() + : +#if _WIN32 + ThreadHandle(NULL), +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + ThreadHandle(0), + Data(NULL), +#else +#error "Unknown System" +#endif + StartRoutine(NULL) + { + } + +#if __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + static void * StartRoutineWrapper(void * data) + { + Impl * impl = (Impl *)data; + + impl->StartRoutine(impl->Data); + + return NULL; + } + +#endif }; -Thread::Thread( - ThreadStartRoutine startRoutine) : - _pimpl(new Thread::Impl()) +Thread::Thread(ThreadStartRoutine startRoutine) : _pimpl(new Thread::Impl()) { - _pimpl->StartRoutine = startRoutine; + _pimpl->StartRoutine = startRoutine; } -Thread::~Thread() -{ - delete _pimpl; -} +Thread::~Thread() { delete _pimpl; } -Thread* Thread::startNew(ThreadStartRoutine startRoutine, void* routineData) +Thread * Thread::startNew(ThreadStartRoutine startRoutine, void * routineData) { - Thread* newThread = new Thread(startRoutine); + Thread * newThread = new Thread(startRoutine); - newThread->start(routineData); + newThread->start(routineData); - return newThread; + return newThread; } -void Thread::start(void* routineData) +void Thread::start(void * routineData) { - if (_pimpl->StartRoutine != NULL) - { - #if _WIN32 - - _pimpl->ThreadHandle = CreateThread( - NULL, - 0, - (LPTHREAD_START_ROUTINE) _pimpl->StartRoutine, - routineData, - 0, - NULL); - - if (_pimpl->ThreadHandle == NULL) - throw unknown_error(); - - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - - _pimpl->Data = routineData; - - int errorCode = pthread_create( - &_pimpl->ThreadHandle, - NULL, - Impl::StartRoutineWrapper, - _pimpl); - - if (errorCode != 0) - throw unknown_error(); - - #else - #error "Unknown System" - #endif - } - else - { - throw not_implemented(); - } + if (_pimpl->StartRoutine != NULL) { +#if _WIN32 + + _pimpl->ThreadHandle = + CreateThread(NULL, 0, (LPTHREAD_START_ROUTINE)_pimpl->StartRoutine, routineData, 0, NULL); + + if (_pimpl->ThreadHandle == NULL) throw unknown_error(); + +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + _pimpl->Data = routineData; + + int errorCode = pthread_create(&_pimpl->ThreadHandle, NULL, Impl::StartRoutineWrapper, _pimpl); + + if (errorCode != 0) throw unknown_error(); + +#else +#error "Unknown System" +#endif + } else { + throw not_implemented(); + } } void Thread::join() { - #if _WIN32 - - WaitForSingleObject( - _pimpl->ThreadHandle, - INFINITE); - - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - - if (pthread_join( - _pimpl->ThreadHandle, - NULL)) - throw unknown_error(); - - #else - #error "Unknown System" - #endif +#if _WIN32 + + WaitForSingleObject(_pimpl->ThreadHandle, INFINITE); + +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + if (pthread_join(_pimpl->ThreadHandle, NULL)) throw unknown_error(); + +#else +#error "Unknown System" +#endif } void Thread::sleepSec(uint32_t numOfSecsToSleep) { - #if _WIN32 - Sleep(numOfSecsToSleep * 1000); - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - sleep(numOfSecsToSleep); - #else - #error "Unknown System" - #endif +#if _WIN32 + Sleep(numOfSecsToSleep * 1000); +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + sleep(numOfSecsToSleep); +#else +#error "Unknown System" +#endif } void Thread::sleepMs(uint32_t numOfMsToSleep) { - #if _WIN32 - Sleep(numOfMsToSleep); - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - usleep(numOfMsToSleep * 1000); - #else - #error "Unknown System" - #endif +#if _WIN32 + Sleep(numOfMsToSleep); +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + usleep(numOfMsToSleep * 1000); +#else +#error "Unknown System" +#endif } void Thread::sleepUs(uint32_t numOfUsToSleep) { - #if _WIN32 - throw not_implemented(); - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - usleep(numOfUsToSleep); - #else - #error "Unknown System" - #endif +#if _WIN32 + throw not_implemented(); +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + usleep(numOfUsToSleep); +#else +#error "Unknown System" +#endif } void Thread::sleepNs(uint32_t numOfNsToSleep) { - #if _WIN32 - throw not_implemented(); - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - throw not_implemented(); - #endif +#if _WIN32 + throw not_implemented(); +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + throw not_implemented(); +#endif } -} -} +} // namespace xplat +} // namespace vn diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/types.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/types.cpp index b8c8f0fe..a14f5a53 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/types.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/types.cpp @@ -1,57 +1,42 @@ #include "vn/types.h" -namespace vn { -namespace protocol { -namespace uart { - -VpeStatus::VpeStatus() +namespace vn { -} - -VpeStatus::VpeStatus(uint16_t raw) +namespace protocol +{ +namespace uart { - attitudeQuality = 0x0003 & raw; - gyroSaturation = (0x0004 & raw) != 0; - gyroSaturationRecovery = (0x0008 & raw) != 0; - magDisturbance = (0x0030 & raw) >> 4; - magSaturation = (0x0040 & raw) != 0; - accDisturbance = (0x0180 & raw) >> 7; - accSaturation = (0x0200 & raw) != 0; - knownMagDisturbance = (0x0800 & raw) != 0; - knownAccelDisturbance = (0x1000 & raw) != 0; -} +VpeStatus::VpeStatus() {} -CommonGroup operator|(CommonGroup lhs, CommonGroup rhs) +VpeStatus::VpeStatus(uint16_t raw) { - return CommonGroup(int(lhs) | int(rhs)); + attitudeQuality = 0x0003 & raw; + gyroSaturation = (0x0004 & raw) != 0; + gyroSaturationRecovery = (0x0008 & raw) != 0; + magDisturbance = (0x0030 & raw) >> 4; + magSaturation = (0x0040 & raw) != 0; + accDisturbance = (0x0180 & raw) >> 7; + accSaturation = (0x0200 & raw) != 0; + knownMagDisturbance = (0x0800 & raw) != 0; + knownAccelDisturbance = (0x1000 & raw) != 0; } -TimeGroup operator|(TimeGroup lhs, TimeGroup rhs) -{ - return TimeGroup(int(lhs) | int(rhs)); -} +CommonGroup operator|(CommonGroup lhs, CommonGroup rhs) { return CommonGroup(int(lhs) | int(rhs)); } -ImuGroup operator|(ImuGroup lhs, ImuGroup rhs) -{ - return ImuGroup(int(lhs) | int(rhs)); -} +TimeGroup operator|(TimeGroup lhs, TimeGroup rhs) { return TimeGroup(int(lhs) | int(rhs)); } -GpsGroup operator|(GpsGroup lhs, GpsGroup rhs) -{ - return GpsGroup(int(lhs) | int(rhs)); -} +ImuGroup operator|(ImuGroup lhs, ImuGroup rhs) { return ImuGroup(int(lhs) | int(rhs)); } + +GpsGroup operator|(GpsGroup lhs, GpsGroup rhs) { return GpsGroup(int(lhs) | int(rhs)); } AttitudeGroup operator|(AttitudeGroup lhs, AttitudeGroup rhs) { - return AttitudeGroup(int(lhs) | int(rhs)); + return AttitudeGroup(int(lhs) | int(rhs)); } -InsGroup operator|(InsGroup lhs, InsGroup rhs) -{ - return InsGroup(int(lhs) | int(rhs)); -} +InsGroup operator|(InsGroup lhs, InsGroup rhs) { return InsGroup(int(lhs) | int(rhs)); } -} -} -} +} // namespace uart +} // namespace protocol +} // namespace vn diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/util.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/util.cpp index c6a78b31..03168f91 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/util.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/util.cpp @@ -4,781 +4,753 @@ using namespace std; -namespace vn { -namespace protocol { -namespace uart { +namespace vn +{ +namespace protocol +{ +namespace uart +{ -char* vnstrtok(char* str, size_t& startIndex); +char * vnstrtok(char * str, size_t & startIndex); string str(AsciiAsync val) { - switch (val) - { - case VNOFF: - return "OFF"; - case VNYPR: - return "VNYPR"; - case VNQTN: - return "VNQTN"; - #ifdef INTERNAL - case VNQTM: - return "VNQTM"; - case VNQTA: - return "VNQTA"; - case VNQTR: - return "VNQTR"; - case VNQMA: - return "VNQMA"; - case VNQAR: - return "VNQAR"; - #endif - case VNQMR: - return "VNQMR"; - #ifdef INTERNAL - case VNDCM: - return "VNDCM"; - #endif - case VNMAG: - return "VNMAG"; - case VNACC: - return "VNACC"; - case VNGYR: - return "VNGYR"; - case VNMAR: - return "VNMAR"; - case VNYMR: - return "VNYMR"; - #ifdef INTERNAL - case VNYCM: - return "VNYCM"; - #endif - case VNYBA: - return "VNYBA"; - case VNYIA: - return "VNYIA"; - #ifdef INTERNAL - case VNICM: - return "VNICM"; - #endif - case VNIMU: - return "VNIMU"; - case VNGPS: - return "VNGPS"; - case VNGPE: - return "VNGPE"; - case VNINS: - return "VNINS"; - case VNINE: - return "VNINE"; - case VNISL: - return "VNISL"; - case VNISE: - return "VNISE"; - case VNDTV: - return "VNDTV"; - #ifdef INTERNAL - case VNRAW: - return "VNRAW"; - case VNCMV: - return "VNCMV"; - case VNSTV: - return "VNSTV"; - case VNCOV: - return "VNCOV"; - #endif - default: - throw invalid_argument("val"); - } -} - -ostream& operator<<(ostream& out, AsciiAsync e) -{ - out << str(e); - return out; + switch (val) { + case VNOFF: + return "OFF"; + case VNYPR: + return "VNYPR"; + case VNQTN: + return "VNQTN"; +#ifdef INTERNAL + case VNQTM: + return "VNQTM"; + case VNQTA: + return "VNQTA"; + case VNQTR: + return "VNQTR"; + case VNQMA: + return "VNQMA"; + case VNQAR: + return "VNQAR"; +#endif + case VNQMR: + return "VNQMR"; +#ifdef INTERNAL + case VNDCM: + return "VNDCM"; +#endif + case VNMAG: + return "VNMAG"; + case VNACC: + return "VNACC"; + case VNGYR: + return "VNGYR"; + case VNMAR: + return "VNMAR"; + case VNYMR: + return "VNYMR"; +#ifdef INTERNAL + case VNYCM: + return "VNYCM"; +#endif + case VNYBA: + return "VNYBA"; + case VNYIA: + return "VNYIA"; +#ifdef INTERNAL + case VNICM: + return "VNICM"; +#endif + case VNIMU: + return "VNIMU"; + case VNGPS: + return "VNGPS"; + case VNGPE: + return "VNGPE"; + case VNINS: + return "VNINS"; + case VNINE: + return "VNINE"; + case VNISL: + return "VNISL"; + case VNISE: + return "VNISE"; + case VNDTV: + return "VNDTV"; +#ifdef INTERNAL + case VNRAW: + return "VNRAW"; + case VNCMV: + return "VNCMV"; + case VNSTV: + return "VNSTV"; + case VNCOV: + return "VNCOV"; +#endif + default: + throw invalid_argument("val"); + } +} + +ostream & operator<<(ostream & out, AsciiAsync e) +{ + out << str(e); + return out; } string str(SensorError val) { - switch (val) - { - case ERR_HARD_FAULT: - return "HardFault"; - case ERR_SERIAL_BUFFER_OVERFLOW: - return "SerialBufferOverflow"; - case ERR_INVALID_CHECKSUM: - return "InvalidChecksum"; - case ERR_INVALID_COMMAND: - return "InvalidCommand"; - case ERR_NOT_ENOUGH_PARAMETERS: - return "NotEnoughParameters"; - case ERR_TOO_MANY_PARAMETERS: - return "TooManyParameters"; - case ERR_INVALID_PARAMETER: - return "InvalidParameter"; - case ERR_INVALID_REGISTER: - return "InvalidRegister"; - case ERR_UNAUTHORIZED_ACCESS: - return "UnauthorizedAccess"; - case ERR_WATCHDOG_RESET: - return "WatchdogReset"; - case ERR_OUTPUT_BUFFER_OVERFLOW: - return "OutputBufferOverflow"; - case ERR_INSUFFICIENT_BAUD_RATE: - return "InsufficientBaudRate"; - case ERR_ERROR_BUFFER_OVERFLOW: - return "ErrorBufferOverflow"; - default: - throw invalid_argument("val"); - } + switch (val) { + case ERR_HARD_FAULT: + return "HardFault"; + case ERR_SERIAL_BUFFER_OVERFLOW: + return "SerialBufferOverflow"; + case ERR_INVALID_CHECKSUM: + return "InvalidChecksum"; + case ERR_INVALID_COMMAND: + return "InvalidCommand"; + case ERR_NOT_ENOUGH_PARAMETERS: + return "NotEnoughParameters"; + case ERR_TOO_MANY_PARAMETERS: + return "TooManyParameters"; + case ERR_INVALID_PARAMETER: + return "InvalidParameter"; + case ERR_INVALID_REGISTER: + return "InvalidRegister"; + case ERR_UNAUTHORIZED_ACCESS: + return "UnauthorizedAccess"; + case ERR_WATCHDOG_RESET: + return "WatchdogReset"; + case ERR_OUTPUT_BUFFER_OVERFLOW: + return "OutputBufferOverflow"; + case ERR_INSUFFICIENT_BAUD_RATE: + return "InsufficientBaudRate"; + case ERR_ERROR_BUFFER_OVERFLOW: + return "ErrorBufferOverflow"; + default: + throw invalid_argument("val"); + } } string str(BootloaderError val) { - switch (val) - { - case BLE_NONE: - return "No Error"; - case BLE_INVALID_COMMAND: - return "Problem with VNX record, abort"; - case BLE_INVALID_RECORD_TYPE: - return "Problem with VNX record, abort"; - case BLE_INVALID_BYTE_COUNT: - return "Problem with VNX record, abort"; - case BLE_INVALID_MEMORY_ADDRESS: - return "Problem with VNX record, abort"; - case BLE_COMM_ERROR: - return "Checksum error, resend record"; - case BLE_INVALID_HEX_FILE: - return "Problem with VNX record, abort"; - case BLE_DECRYPTION_ERROR: - return "Invalid VNX file or record sent out of order, abort"; - case BLE_INVALID_BLOCK_CRC: - return "Data verification failed, abort"; - case BLE_INVALID_PROGRAM_CRC: - return "Problem with firmware on device"; - case BLE_INVALID_PROGRAM_SIZE: - return "Problem with firmware on device"; - case BLE_MAX_RETRY_COUNT: - return "Too many errors, abort"; - case BLE_TIMEOUT: - return "Timeout expired, reset"; - case BLE_RESERVED: - return "Contact VectorNav, abort"; - default: - throw invalid_argument("val"); - } -} - -ostream& operator<<(ostream& out, SensorError e) -{ - out << str(e); - return out; + switch (val) { + case BLE_NONE: + return "No Error"; + case BLE_INVALID_COMMAND: + return "Problem with VNX record, abort"; + case BLE_INVALID_RECORD_TYPE: + return "Problem with VNX record, abort"; + case BLE_INVALID_BYTE_COUNT: + return "Problem with VNX record, abort"; + case BLE_INVALID_MEMORY_ADDRESS: + return "Problem with VNX record, abort"; + case BLE_COMM_ERROR: + return "Checksum error, resend record"; + case BLE_INVALID_HEX_FILE: + return "Problem with VNX record, abort"; + case BLE_DECRYPTION_ERROR: + return "Invalid VNX file or record sent out of order, abort"; + case BLE_INVALID_BLOCK_CRC: + return "Data verification failed, abort"; + case BLE_INVALID_PROGRAM_CRC: + return "Problem with firmware on device"; + case BLE_INVALID_PROGRAM_SIZE: + return "Problem with firmware on device"; + case BLE_MAX_RETRY_COUNT: + return "Too many errors, abort"; + case BLE_TIMEOUT: + return "Timeout expired, reset"; + case BLE_RESERVED: + return "Contact VectorNav, abort"; + default: + throw invalid_argument("val"); + } +} + +ostream & operator<<(ostream & out, SensorError e) +{ + out << str(e); + return out; } string str(SyncInMode val) { - switch (val) - { - #ifdef INTERNAL - case SYNCINMODE_COUNT2: - return "Count2"; - case SYNCINMODE_ADC2: - return "Adc2"; - case SYNCINMODE_ASYNC2: - return "Async2"; - #endif - case SYNCINMODE_COUNT: - return "Count"; - case SYNCINMODE_IMU: - return "Imu"; - case SYNCINMODE_ASYNC: - return "Async"; - case SYNCINMODE_ASYNC3: - return "Async3"; - default: - throw invalid_argument("val"); - } + switch (val) { +#ifdef INTERNAL + case SYNCINMODE_COUNT2: + return "Count2"; + case SYNCINMODE_ADC2: + return "Adc2"; + case SYNCINMODE_ASYNC2: + return "Async2"; +#endif + case SYNCINMODE_COUNT: + return "Count"; + case SYNCINMODE_IMU: + return "Imu"; + case SYNCINMODE_ASYNC: + return "Async"; + case SYNCINMODE_ASYNC3: + return "Async3"; + default: + throw invalid_argument("val"); + } } string str(SyncInEdge val) { - switch (val) - { - case SYNCINEDGE_RISING: - return "Rising"; - case SYNCINEDGE_FALLING: - return "Falling"; - default: - throw invalid_argument("val"); - } + switch (val) { + case SYNCINEDGE_RISING: + return "Rising"; + case SYNCINEDGE_FALLING: + return "Falling"; + default: + throw invalid_argument("val"); + } } string str(SyncOutMode val) { - switch (val) - { - case SYNCOUTMODE_NONE: - return "None"; - case SYNCOUTMODE_ITEMSTART: - return "ItemStart"; - case SYNCOUTMODE_IMUREADY: - return "ImuReady"; - case SYNCOUTMODE_INS: - return "Ins"; - case SYNCOUTMODE_GPSPPS: - return "GpsPps"; - default: - throw invalid_argument("val"); - } + switch (val) { + case SYNCOUTMODE_NONE: + return "None"; + case SYNCOUTMODE_ITEMSTART: + return "ItemStart"; + case SYNCOUTMODE_IMUREADY: + return "ImuReady"; + case SYNCOUTMODE_INS: + return "Ins"; + case SYNCOUTMODE_GPSPPS: + return "GpsPps"; + default: + throw invalid_argument("val"); + } } string str(SyncOutPolarity val) { - switch (val) - { - case SYNCOUTPOLARITY_NEGATIVE: - return "Negative"; - case SYNCOUTPOLARITY_POSITIVE: - return "Positive"; - default: - throw invalid_argument("val"); - } + switch (val) { + case SYNCOUTPOLARITY_NEGATIVE: + return "Negative"; + case SYNCOUTPOLARITY_POSITIVE: + return "Positive"; + default: + throw invalid_argument("val"); + } } string str(CountMode val) { - switch (val) - { - case COUNTMODE_NONE: - return "None"; - case COUNTMODE_SYNCINCOUNT: - return "SyncInCount"; - case COUNTMODE_SYNCINTIME: - return "SyncInTime"; - case COUNTMODE_SYNCOUTCOUNTER: - return "SyncOutCounter"; - case COUNTMODE_GPSPPS: - return "GpsPps"; - default: - throw invalid_argument("val"); - } + switch (val) { + case COUNTMODE_NONE: + return "None"; + case COUNTMODE_SYNCINCOUNT: + return "SyncInCount"; + case COUNTMODE_SYNCINTIME: + return "SyncInTime"; + case COUNTMODE_SYNCOUTCOUNTER: + return "SyncOutCounter"; + case COUNTMODE_GPSPPS: + return "GpsPps"; + default: + throw invalid_argument("val"); + } } string str(StatusMode val) { - switch (val) - { - case STATUSMODE_OFF: - return "Off"; - case STATUSMODE_VPESTATUS: - return "VpeStatus"; - case STATUSMODE_INSSTATUS: - return "InsStatus"; - default: - throw invalid_argument("val"); - } + switch (val) { + case STATUSMODE_OFF: + return "Off"; + case STATUSMODE_VPESTATUS: + return "VpeStatus"; + case STATUSMODE_INSSTATUS: + return "InsStatus"; + default: + throw invalid_argument("val"); + } } string str(ChecksumMode val) { - switch (val) - { - case CHECKSUMMODE_OFF: - return "Off"; - case CHECKSUMMODE_CHECKSUM: - return "Checksum"; - case CHECKSUMMODE_CRC: - return "Crc"; - default: - throw invalid_argument("val"); - } + switch (val) { + case CHECKSUMMODE_OFF: + return "Off"; + case CHECKSUMMODE_CHECKSUM: + return "Checksum"; + case CHECKSUMMODE_CRC: + return "Crc"; + default: + throw invalid_argument("val"); + } } string str(ErrorMode val) { - switch (val) - { - case ERRORMODE_IGNORE: - return "Ignore"; - case ERRORMODE_SEND: - return "Send"; - case ERRORMODE_SENDANDOFF: - return "SendAndOff"; - default: - throw invalid_argument("val"); - } + switch (val) { + case ERRORMODE_IGNORE: + return "Ignore"; + case ERRORMODE_SEND: + return "Send"; + case ERRORMODE_SENDANDOFF: + return "SendAndOff"; + default: + throw invalid_argument("val"); + } } string str(FilterMode val) { - switch (val) - { - case FILTERMODE_NOFILTERING: - return "NoFiltering"; - case FILTERMODE_ONLYRAW: - return "OnlyRaw"; - case FILTERMODE_ONLYCOMPENSATED: - return "OnlyCompensated"; - case FILTERMODE_BOTH: - return "Both"; - default: - throw invalid_argument("val"); - } + switch (val) { + case FILTERMODE_NOFILTERING: + return "NoFiltering"; + case FILTERMODE_ONLYRAW: + return "OnlyRaw"; + case FILTERMODE_ONLYCOMPENSATED: + return "OnlyCompensated"; + case FILTERMODE_BOTH: + return "Both"; + default: + throw invalid_argument("val"); + } } string str(IntegrationFrame val) { - switch (val) - { - case INTEGRATIONFRAME_BODY: - return "Body"; - case INTEGRATIONFRAME_NED: - return "Ned"; - default: - throw invalid_argument("val"); - } + switch (val) { + case INTEGRATIONFRAME_BODY: + return "Body"; + case INTEGRATIONFRAME_NED: + return "Ned"; + default: + throw invalid_argument("val"); + } } string str(CompensationMode val) { - switch (val) - { - case COMPENSATIONMODE_NONE: - return "None"; - case COMPENSATIONMODE_BIAS: - return "Bias"; - default: - throw invalid_argument("val"); - } + switch (val) { + case COMPENSATIONMODE_NONE: + return "None"; + case COMPENSATIONMODE_BIAS: + return "Bias"; + default: + throw invalid_argument("val"); + } } string str(AccCompensationMode val) { - switch (val) - { - case ACCCOMPENSATIONMODE_NONE: - return "None"; - case ACCCOMPENSATIONMODE_BIAS: - return "Bias"; - case ACCCOMPENSATIONMODE_GRAV: - return "Gravity"; - case ACCCOMPENSATIONMODE_BIASGRAV: - return "Bias & Gravity"; - default: - throw invalid_argument("val"); - } + switch (val) { + case ACCCOMPENSATIONMODE_NONE: + return "None"; + case ACCCOMPENSATIONMODE_BIAS: + return "Bias"; + case ACCCOMPENSATIONMODE_GRAV: + return "Gravity"; + case ACCCOMPENSATIONMODE_BIASGRAV: + return "Bias & Gravity"; + default: + throw invalid_argument("val"); + } } string str(EarthRateCorrection val) { - switch (val) - { - case EARTHRATECORR_NONE: - return "None"; - case EARTHRATECORR_RATE: - return "Angular Rate"; - case EARTHRATECORR_CORIOLIS: - return "Coriolis Acceleration"; - case EARTHRATECORR_RATECOR: - return "Angular Rate & Coriolis"; - default: - throw invalid_argument("val"); - } + switch (val) { + case EARTHRATECORR_NONE: + return "None"; + case EARTHRATECORR_RATE: + return "Angular Rate"; + case EARTHRATECORR_CORIOLIS: + return "Coriolis Acceleration"; + case EARTHRATECORR_RATECOR: + return "Angular Rate & Coriolis"; + default: + throw invalid_argument("val"); + } } string str(GpsFix val) { - switch (val) - { - case GPSFIX_NOFIX: - return "NoFix"; - case GPSFIX_TIMEONLY: - return "TimeOnly"; - case GPSFIX_2D: - return "2D"; - case GPSFIX_3D: - return "3D"; - default: - throw invalid_argument("val"); - } + switch (val) { + case GPSFIX_NOFIX: + return "NoFix"; + case GPSFIX_TIMEONLY: + return "TimeOnly"; + case GPSFIX_2D: + return "2D"; + case GPSFIX_3D: + return "3D"; + default: + throw invalid_argument("val"); + } } string str(GpsMode val) { - switch (val) - { - case GPSMODE_ONBOARDGPS: - return "OnBoardGps"; - case GPSMODE_EXTERNALGPS: - return "ExternalGps"; - case GPSMODE_EXTERNALVN200GPS: - return "ExternalVn200Gps"; - default: - throw invalid_argument("val"); - } + switch (val) { + case GPSMODE_ONBOARDGPS: + return "OnBoardGps"; + case GPSMODE_EXTERNALGPS: + return "ExternalGps"; + case GPSMODE_EXTERNALVN200GPS: + return "ExternalVn200Gps"; + default: + throw invalid_argument("val"); + } } string str(PpsSource val) { - switch (val) - { - case PPSSOURCE_GPSPPSRISING: - return "GpsPpsRising"; - case PPSSOURCE_GPSPPSFALLING: - return "GpsPpsFalling"; - case PPSSOURCE_SYNCINRISING: - return "SyncInRising"; - case PPSSOURCE_SYNCINFALLING: - return "SyncInFalling"; - default: - throw invalid_argument("val"); - } + switch (val) { + case PPSSOURCE_GPSPPSRISING: + return "GpsPpsRising"; + case PPSSOURCE_GPSPPSFALLING: + return "GpsPpsFalling"; + case PPSSOURCE_SYNCINRISING: + return "SyncInRising"; + case PPSSOURCE_SYNCINFALLING: + return "SyncInFalling"; + default: + throw invalid_argument("val"); + } } string str(GpsRate val) { - switch (val) - { - case GPSRATE_1HZ: - return "Rate1Hz"; - case GPSRATE_5HZ: - return "Rate5Hz"; - default: - throw invalid_argument("val"); - } + switch (val) { + case GPSRATE_1HZ: + return "Rate1Hz"; + case GPSRATE_5HZ: + return "Rate5Hz"; + default: + throw invalid_argument("val"); + } } string str(AntPower val) { - switch (val) - { - case ANTPOWER_OFFRESV: - return "AntPowOffResv"; - case ANTPOWER_INTERNAL: - return "AntPowInternal"; - case ANTPOWER_EXTERNAL: - return "AntPowExternal"; - default: - throw invalid_argument("val"); - } + switch (val) { + case ANTPOWER_OFFRESV: + return "AntPowOffResv"; + case ANTPOWER_INTERNAL: + return "AntPowInternal"; + case ANTPOWER_EXTERNAL: + return "AntPowExternal"; + default: + throw invalid_argument("val"); + } } string str(VpeEnable val) { - switch (val) - { - case VPEENABLE_DISABLE: - return "Disable"; - case VPEENABLE_ENABLE: - return "Enable"; - default: - throw invalid_argument("val"); - } + switch (val) { + case VPEENABLE_DISABLE: + return "Disable"; + case VPEENABLE_ENABLE: + return "Enable"; + default: + throw invalid_argument("val"); + } } string str(HeadingMode val) { - switch (val) - { - case HEADINGMODE_ABSOLUTE: - return "Absolute"; - case HEADINGMODE_RELATIVE: - return "Relative"; - case HEADINGMODE_INDOOR: - return "Indoor"; - default: - throw invalid_argument("val"); - } + switch (val) { + case HEADINGMODE_ABSOLUTE: + return "Absolute"; + case HEADINGMODE_RELATIVE: + return "Relative"; + case HEADINGMODE_INDOOR: + return "Indoor"; + default: + throw invalid_argument("val"); + } } string str(VpeMode val) { - switch (val) - { - case VPEMODE_OFF: - return "Off"; - case VPEMODE_MODE1: - return "Mode1"; - default: - throw invalid_argument("val"); - } + switch (val) { + case VPEMODE_OFF: + return "Off"; + case VPEMODE_MODE1: + return "Mode1"; + default: + throw invalid_argument("val"); + } } string str(Scenario val) { - switch (val) - { - case SCENARIO_AHRS: - return "Ahrs"; - case SCENARIO_INSWITHPRESSURE: - return "InsWithPressure"; - case SCENARIO_INSWITHOUTPRESSURE: - return "InsWithoutPressure"; - case SCENARIO_GPSMOVINGBASELINEDYNAMIC: - return "GpsMovingBaselineDynamic"; - case SCENARIO_GPSMOVINGBASELINESTATIC: - return "GpsMovingBaselineStatic"; - default: - throw invalid_argument("val"); - } + switch (val) { + case SCENARIO_AHRS: + return "Ahrs"; + case SCENARIO_INSWITHPRESSURE: + return "InsWithPressure"; + case SCENARIO_INSWITHOUTPRESSURE: + return "InsWithoutPressure"; + case SCENARIO_GPSMOVINGBASELINEDYNAMIC: + return "GpsMovingBaselineDynamic"; + case SCENARIO_GPSMOVINGBASELINESTATIC: + return "GpsMovingBaselineStatic"; + default: + throw invalid_argument("val"); + } } string str(HsiMode val) { - switch (val) - { - case HSIMODE_OFF: - return "Off"; - case HSIMODE_RUN: - return "Run"; - case HSIMODE_RESET: - return "Reset"; - default: - throw invalid_argument("val"); - } + switch (val) { + case HSIMODE_OFF: + return "Off"; + case HSIMODE_RUN: + return "Run"; + case HSIMODE_RESET: + return "Reset"; + default: + throw invalid_argument("val"); + } } string str(HsiOutput val) { - switch (val) - { - case HSIOUTPUT_NOONBOARD: - return "NoOnboard"; - case HSIOUTPUT_USEONBOARD: - return "UseOnboard"; - default: - throw invalid_argument("val"); - } + switch (val) { + case HSIOUTPUT_NOONBOARD: + return "NoOnboard"; + case HSIOUTPUT_USEONBOARD: + return "UseOnboard"; + default: + throw invalid_argument("val"); + } } string str(VelocityCompensationMode val) { - switch (val) - { - case VELOCITYCOMPENSATIONMODE_DISABLED: - return "Disabled"; - case VELOCITYCOMPENSATIONMODE_BODYMEASUREMENT: - return "BodyMeasurement"; - default: - throw invalid_argument("val"); - } + switch (val) { + case VELOCITYCOMPENSATIONMODE_DISABLED: + return "Disabled"; + case VELOCITYCOMPENSATIONMODE_BODYMEASUREMENT: + return "BodyMeasurement"; + default: + throw invalid_argument("val"); + } } string str(MagneticMode val) { - switch (val) - { - case MAGNETICMODE_2D: - return "2D"; - case MAGNETICMODE_3D: - return "3D"; - default: - throw invalid_argument("val"); - } + switch (val) { + case MAGNETICMODE_2D: + return "2D"; + case MAGNETICMODE_3D: + return "3D"; + default: + throw invalid_argument("val"); + } } string str(ExternalSensorMode val) { - switch (val) - { - case EXTERNALSENSORMODE_INTERNAL: - return "Internal"; - case EXTERNALSENSORMODE_EXTERNAL200HZ: - return "External200Hz"; - case EXTERNALSENSORMODE_EXTERNALONUPDATE: - return "ExternalOnUpdate"; - default: - throw invalid_argument("val"); - } + switch (val) { + case EXTERNALSENSORMODE_INTERNAL: + return "Internal"; + case EXTERNALSENSORMODE_EXTERNAL200HZ: + return "External200Hz"; + case EXTERNALSENSORMODE_EXTERNALONUPDATE: + return "ExternalOnUpdate"; + default: + throw invalid_argument("val"); + } } string str(FoamInit val) { - switch (val) - { - case FOAMINIT_NOFOAMINIT: - return "NoFoamInit"; - case FOAMINIT_FOAMINITPITCHROLL: - return "FoamInitPitchRoll"; - case FOAMINIT_FOAMINITHEADINGPITCHROLL: - return "FoamInitHeadingPitchRoll"; - case FOAMINIT_FOAMINITPITCHROLLCOVARIANCE: - return "FoamInitPitchRollCovariance"; - case FOAMINIT_FOAMINITHEADINGPITCHROLLCOVARIANCE: - return "FoamInitHeadingPitchRollCovariance"; - default: - throw invalid_argument("val"); - } + switch (val) { + case FOAMINIT_NOFOAMINIT: + return "NoFoamInit"; + case FOAMINIT_FOAMINITPITCHROLL: + return "FoamInitPitchRoll"; + case FOAMINIT_FOAMINITHEADINGPITCHROLL: + return "FoamInitHeadingPitchRoll"; + case FOAMINIT_FOAMINITPITCHROLLCOVARIANCE: + return "FoamInitPitchRollCovariance"; + case FOAMINIT_FOAMINITHEADINGPITCHROLLCOVARIANCE: + return "FoamInitHeadingPitchRollCovariance"; + default: + throw invalid_argument("val"); + } } -ostream& operator<<(ostream& out, SyncInMode e) +ostream & operator<<(ostream & out, SyncInMode e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, SyncInEdge e) +ostream & operator<<(ostream & out, SyncInEdge e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, SyncOutMode e) +ostream & operator<<(ostream & out, SyncOutMode e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, SyncOutPolarity e) +ostream & operator<<(ostream & out, SyncOutPolarity e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, CountMode e) +ostream & operator<<(ostream & out, CountMode e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, StatusMode e) +ostream & operator<<(ostream & out, StatusMode e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, ChecksumMode e) +ostream & operator<<(ostream & out, ChecksumMode e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, ErrorMode e) +ostream & operator<<(ostream & out, ErrorMode e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, FilterMode e) +ostream & operator<<(ostream & out, FilterMode e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, IntegrationFrame e) +ostream & operator<<(ostream & out, IntegrationFrame e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, CompensationMode e) +ostream & operator<<(ostream & out, CompensationMode e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, AccCompensationMode e) +ostream & operator<<(ostream & out, AccCompensationMode e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, EarthRateCorrection e) +ostream & operator<<(ostream & out, EarthRateCorrection e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, GpsFix e) +ostream & operator<<(ostream & out, GpsFix e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, GpsMode e) +ostream & operator<<(ostream & out, GpsMode e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, PpsSource e) +ostream & operator<<(ostream & out, PpsSource e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, GpsRate e) +ostream & operator<<(ostream & out, GpsRate e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, AntPower e) +ostream & operator<<(ostream & out, AntPower e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, VpeEnable e) +ostream & operator<<(ostream & out, VpeEnable e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, HeadingMode e) +ostream & operator<<(ostream & out, HeadingMode e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, VpeMode e) +ostream & operator<<(ostream & out, VpeMode e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, Scenario e) +ostream & operator<<(ostream & out, Scenario e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, HsiMode e) +ostream & operator<<(ostream & out, HsiMode e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, HsiOutput e) +ostream & operator<<(ostream & out, HsiOutput e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, VelocityCompensationMode e) +ostream & operator<<(ostream & out, VelocityCompensationMode e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, MagneticMode e) +ostream & operator<<(ostream & out, MagneticMode e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, ExternalSensorMode e) +ostream & operator<<(ostream & out, ExternalSensorMode e) { - out << str(e); - return out; + out << str(e); + return out; } -ostream& operator<<(ostream& out, FoamInit e) +ostream & operator<<(ostream & out, FoamInit e) { - out << str(e); - return out; + out << str(e); + return out; } -} -} -} +} // namespace uart +} // namespace protocol +} // namespace vn diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/utilities.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/utilities.cpp index 119e21d4..e74634f5 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/utilities.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/utilities.cpp @@ -1,202 +1,186 @@ -#include "vn/export.h" #include "vn/utilities.h" #include +#include "vn/export.h" + #if defined _WINDOWS && PYTHON - // TODO : This needs to function in Linux as well - #include "dllvalidator.h" +// TODO : This needs to function in Linux as well +#include "dllvalidator.h" #else - #if defined __linux__ - #endif +#if defined __linux__ +#endif #endif #if _M_IX86 || __i386__ || __x86_64 || _WIN64 - // Compiling for x86 processor. - #define VN_LITTLE_ENDIAN 1 +// Compiling for x86 processor. +#define VN_LITTLE_ENDIAN 1 #elif __linux__ - // Don't know what processor we are compiling for but we have endian.h. - #define VN_HAVE_ENDIAN_H 1 - #include +// Don't know what processor we are compiling for but we have endian.h. +#define VN_HAVE_ENDIAN_H 1 +#include #elif __BYTE_ORDER__ - #if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ - #define VN_LITTLE_ENDIAN 1 - #elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__ - #define VN_BIG_ENDIAN 1 - #else - #error "Unknown System" - #endif +#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ +#define VN_LITTLE_ENDIAN 1 +#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__ +#define VN_BIG_ENDIAN 1 #else - #error "Unknown System" +#error "Unknown System" +#endif +#else +#error "Unknown System" #endif using namespace std; -namespace vn { - -int ApiVersion::major() +namespace vn { - return VNAPI_MAJOR; -} -int ApiVersion::minor() -{ - return VNAPI_MINOR; -} +int ApiVersion::major() { return VNAPI_MAJOR; } -int ApiVersion::patch() -{ - return VNAPI_PATCH; -} +int ApiVersion::minor() { return VNAPI_MINOR; } -int ApiVersion::revision() -{ - return VNAPI_REVISION; -} +int ApiVersion::patch() { return VNAPI_PATCH; } + +int ApiVersion::revision() { return VNAPI_REVISION; } string ApiVersion::getVersion() { - stringstream ss; + stringstream ss; - ss << major() << "." << minor() << "." << patch() << "." << revision(); + ss << major() << "." << minor() << "." << patch() << "." << revision(); - return ss.str(); + return ss.str(); } -uint8_t toUint8FromHexStr(char const* str) +uint8_t toUint8FromHexStr(char const * str) { - uint8_t result; + uint8_t result; - result = to_uint8_from_hexchar(str[0]) << 4; - result += to_uint8_from_hexchar(str[1]); + result = to_uint8_from_hexchar(str[0]) << 4; + result += to_uint8_from_hexchar(str[1]); - return result; + return result; } uint16_t stoh(uint16_t sensorOrdered) { - #if VN_LITTLE_ENDIAN - return sensorOrdered; - #elif VN_BIG_ENDIAN - uint16_t host; - host = ((sensorOrdered >> 0) & 0xFF) * 0x0100; - host += ((sensorOrdered >> 8) & 0xFF) * 0x0001; - return host; - #elif VN_HAVE_ENDIAN_H - return le16toh(sensorOrdered); - #else - #error("Unknown system") - #endif +#if VN_LITTLE_ENDIAN + return sensorOrdered; +#elif VN_BIG_ENDIAN + uint16_t host; + host = ((sensorOrdered >> 0) & 0xFF) * 0x0100; + host += ((sensorOrdered >> 8) & 0xFF) * 0x0001; + return host; +#elif VN_HAVE_ENDIAN_H + return le16toh(sensorOrdered); +#else +#error("Unknown system") +#endif } uint32_t stoh(uint32_t sensorOrdered) { - #if VN_LITTLE_ENDIAN - return sensorOrdered; - #elif VN_BIG_ENDIAN - uint32_t host; - host = ((sensorOrdered >> 0) & 0xFF) * 0x01000000; - host += ((sensorOrdered >> 8) & 0xFF) * 0x00010000; - host += ((sensorOrdered >> 16) & 0xFF) * 0x00000100; - host += ((sensorOrdered >> 24) & 0xFF) * 0x00000001; - return host; - #elif VN_HAVE_ENDIAN_H - return le32toh(sensorOrdered); - #else - #error("Unknown system") - #endif +#if VN_LITTLE_ENDIAN + return sensorOrdered; +#elif VN_BIG_ENDIAN + uint32_t host; + host = ((sensorOrdered >> 0) & 0xFF) * 0x01000000; + host += ((sensorOrdered >> 8) & 0xFF) * 0x00010000; + host += ((sensorOrdered >> 16) & 0xFF) * 0x00000100; + host += ((sensorOrdered >> 24) & 0xFF) * 0x00000001; + return host; +#elif VN_HAVE_ENDIAN_H + return le32toh(sensorOrdered); +#else +#error("Unknown system") +#endif } uint64_t stoh(uint64_t sensorOrdered) { - #if VN_LITTLE_ENDIAN - return sensorOrdered; - #elif VN_BIG_ENDIAN - uint64_t host; - host = ((sensorOrdered >> 0) & 0xFF) * 0x0100000000000000; - host += ((sensorOrdered >> 8) & 0xFF) * 0x0001000000000000; - host += ((sensorOrdered >> 16) & 0xFF) * 0x0000010000000000; - host += ((sensorOrdered >> 24) & 0xFF) * 0x0000000100000000; - host += ((sensorOrdered >> 32) & 0xFF) * 0x0000000001000000; - host += ((sensorOrdered >> 40) & 0xFF) * 0x0000000000010000; - host += ((sensorOrdered >> 48) & 0xFF) * 0x0000000000000100; - host += ((sensorOrdered >> 56) & 0xFF) * 0x0000000000000001; - return host; - #elif VN_HAVE_ENDIAN_H - return le64toh(sensorOrdered); - #else - #error("Unknown system") - #endif +#if VN_LITTLE_ENDIAN + return sensorOrdered; +#elif VN_BIG_ENDIAN + uint64_t host; + host = ((sensorOrdered >> 0) & 0xFF) * 0x0100000000000000; + host += ((sensorOrdered >> 8) & 0xFF) * 0x0001000000000000; + host += ((sensorOrdered >> 16) & 0xFF) * 0x0000010000000000; + host += ((sensorOrdered >> 24) & 0xFF) * 0x0000000100000000; + host += ((sensorOrdered >> 32) & 0xFF) * 0x0000000001000000; + host += ((sensorOrdered >> 40) & 0xFF) * 0x0000000000010000; + host += ((sensorOrdered >> 48) & 0xFF) * 0x0000000000000100; + host += ((sensorOrdered >> 56) & 0xFF) * 0x0000000000000001; + return host; +#elif VN_HAVE_ENDIAN_H + return le64toh(sensorOrdered); +#else +#error("Unknown system") +#endif } uint8_t countSetBits(uint8_t d) { - uint8_t count = 0; + uint8_t count = 0; - while (d) - { - d &= (d - 1); - count++; - } + while (d) { + d &= (d - 1); + count++; + } - return count; + return count; } - - uint8_t to_uint8_from_hexchar(char c) { - if (c < ':') - return c - '0'; + if (c < ':') return c - '0'; - if (c < 'G') - return c - '7'; + if (c < 'G') return c - '7'; - return c - 'W'; + return c - 'W'; } -uint8_t to_uint8_from_hexstr(char const* str) -{ - return toUint8FromHexStr(str); -} +uint8_t to_uint8_from_hexstr(char const * str) { return toUint8FromHexStr(str); } -uint16_t to_uint16_from_hexstr(char const* str) +uint16_t to_uint16_from_hexstr(char const * str) { - uint16_t result; + uint16_t result; - result = to_uint8_from_hexstr(str) << 8; - result += to_uint8_from_hexstr(str + 2); + result = to_uint8_from_hexstr(str) << 8; + result += to_uint8_from_hexstr(str + 2); - return result; + return result; } #if PYTHON -bool checkDllValidity(const std::string& dllName, const std::string& workingDirectory, std::vector& missingDlls) +bool checkDllValidity( + const std::string & dllName, const std::string & workingDirectory, + std::vector & missingDlls) { - #if defined _WINDOWS - bool isValid = false; - - missingDlls.clear(); - - DllValidator validator(dllName, workingDirectory); - validator.initialize(); - isValid = validator.validate(); - validator.getMissingDllNames(missingDlls); - - return isValid; - #else - #if defined _LINUX - // TODO : To avoid lots of rework I am simply making this return true if _LINUX is defined - // Once this is implemented for Linux rework will be minimal - return true; - #else - // TODO : Any other systems that need to implement this will go here. - return true; - #endif - #endif +#if defined _WINDOWS + bool isValid = false; + + missingDlls.clear(); + + DllValidator validator(dllName, workingDirectory); + validator.initialize(); + isValid = validator.validate(); + validator.getMissingDllNames(missingDlls); + + return isValid; +#else +#if defined _LINUX + // TODO : To avoid lots of rework I am simply making this return true if _LINUX is defined + // Once this is implemented for Linux rework will be minimal + return true; +#else + // TODO : Any other systems that need to implement this will go here. + return true; +#endif +#endif } #endif -} +} // namespace vn diff --git a/vectornav/vnproglib-1.2.0.0/cpp/src/vntime.cpp b/vectornav/vnproglib-1.2.0.0/cpp/src/vntime.cpp index ab27948d..d80cb9cb 100644 --- a/vectornav/vnproglib-1.2.0.0/cpp/src/vntime.cpp +++ b/vectornav/vnproglib-1.2.0.0/cpp/src/vntime.cpp @@ -1,183 +1,171 @@ #include "vn/vntime.h" #if _WIN32 - #include +#include #elif __linux__ || __CYGWIN__ || __QNXNTO__ - #include - #include +#include +#include #elif __APPLE__ - #include - #include - #include - #include +#include +#include +#include +#include #else - #error "Unknown System" +#error "Unknown System" #endif #include "vn/exceptions.h" using namespace std; -namespace vn { -namespace xplat { +namespace vn +{ +namespace xplat +{ -TimeStamp::TimeStamp() : _sec(0), _usec(0) { } +TimeStamp::TimeStamp() : _sec(0), _usec(0) {} -TimeStamp::TimeStamp(int64_t sec, uint64_t usec) : _sec(sec), _usec(usec) { } +TimeStamp::TimeStamp(int64_t sec, uint64_t usec) : _sec(sec), _usec(usec) {} TimeStamp TimeStamp::get() { - #if _WIN32 - - // HACK: Just returning an empty TimeStamp for now. - return TimeStamp(); - - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - - struct timeval tv; - - gettimeofday(&tv, NULL); - - return TimeStamp(tv.tv_sec, tv.tv_usec); - - #else - #error "Unknown System" - #endif +#if _WIN32 + + // HACK: Just returning an empty TimeStamp for now. + return TimeStamp(); + +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + struct timeval tv; + + gettimeofday(&tv, NULL); + + return TimeStamp(tv.tv_sec, tv.tv_usec); + +#else +#error "Unknown System" +#endif } struct Stopwatch::Impl { - #if _WIN32 - double _pcFrequency; - __int64 _counterStart; - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - double _clockStart; - #else - #error "Unknown System" - #endif - - Impl() : - #if _WIN32 - _pcFrequency(0), - _counterStart(-1) - #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ - _clockStart(-1) - #else - #error "Unknown System" - #endif - { - // Start the stopwatch. - reset(); - } - - void reset() - { - #if _WIN32 - - LARGE_INTEGER li; - if(!QueryPerformanceFrequency(&li)) - // The hardware must not support a high-resolution performance counter. - throw not_supported(); - - _pcFrequency = static_cast(li.QuadPart) / 1000.0; - - QueryPerformanceCounter(&li); - - _counterStart = li.QuadPart; - - #elif __linux__ || __CYGWIN__ || __QNXNTO__ - - struct timespec time; - int error; - - error = clock_gettime(CLOCK_MONOTONIC, &time); - - if (error) - throw unknown_error(); - - _clockStart = (time.tv_sec * 1000.0) + (time.tv_nsec / 1000000.0); - - #elif __APPLE__ - - clock_serv_t cclock; - mach_timespec_t mts; - - host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock); - clock_get_time(cclock, &mts); - mach_port_deallocate(mach_task_self(), cclock); - - _clockStart = (mts.tv_sec * 1000.0) + (mts.tv_nsec / 1000000.0); - - #else - #error "Unknown System" - #endif - } - - float elapsedMs() - { - #if _WIN32 - - LARGE_INTEGER li; - - if (_counterStart == -1) - throw unknown_error(); - - QueryPerformanceCounter(&li); - - return static_cast((static_cast(li.QuadPart) - _counterStart) / _pcFrequency); - - #elif __linux__ || __CYGWIN__ || __QNXNTO__ - - struct timespec time; - int error; - - if (_clockStart < 0) - // Clock not started. - throw invalid_operation(); - - error = clock_gettime(CLOCK_MONOTONIC, &time); - - if (error) - throw unknown_error(); - - return (time.tv_sec * 1000.0) + (time.tv_nsec / 1000000.0) - _clockStart; - - #elif __APPLE__ - - clock_serv_t cclock; - mach_timespec_t mts; - - host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock); - clock_get_time(cclock, &mts); - mach_port_deallocate(mach_task_self(), cclock); - - return (mts.tv_sec * 1000.0) + (mts.tv_nsec / 1000000.0) - _clockStart; - - #else - #error "Unknown System" - #endif - } +#if _WIN32 + double _pcFrequency; + __int64 _counterStart; +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + double _clockStart; +#else +#error "Unknown System" +#endif + + Impl() + : +#if _WIN32 + _pcFrequency(0), + _counterStart(-1) +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + _clockStart(-1) +#else +#error "Unknown System" +#endif + { + // Start the stopwatch. + reset(); + } + + void reset() + { +#if _WIN32 + + LARGE_INTEGER li; + if (!QueryPerformanceFrequency(&li)) + // The hardware must not support a high-resolution performance counter. + throw not_supported(); + + _pcFrequency = static_cast(li.QuadPart) / 1000.0; + + QueryPerformanceCounter(&li); + + _counterStart = li.QuadPart; + +#elif __linux__ || __CYGWIN__ || __QNXNTO__ + + struct timespec time; + int error; + + error = clock_gettime(CLOCK_MONOTONIC, &time); + + if (error) throw unknown_error(); + + _clockStart = (time.tv_sec * 1000.0) + (time.tv_nsec / 1000000.0); + +#elif __APPLE__ + + clock_serv_t cclock; + mach_timespec_t mts; + + host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock); + clock_get_time(cclock, &mts); + mach_port_deallocate(mach_task_self(), cclock); + + _clockStart = (mts.tv_sec * 1000.0) + (mts.tv_nsec / 1000000.0); + +#else +#error "Unknown System" +#endif + } + + float elapsedMs() + { +#if _WIN32 + + LARGE_INTEGER li; + + if (_counterStart == -1) throw unknown_error(); + + QueryPerformanceCounter(&li); + + return static_cast((static_cast(li.QuadPart) - _counterStart) / _pcFrequency); + +#elif __linux__ || __CYGWIN__ || __QNXNTO__ + + struct timespec time; + int error; + + if (_clockStart < 0) + // Clock not started. + throw invalid_operation(); + + error = clock_gettime(CLOCK_MONOTONIC, &time); + + if (error) throw unknown_error(); + + return (time.tv_sec * 1000.0) + (time.tv_nsec / 1000000.0) - _clockStart; + +#elif __APPLE__ + + clock_serv_t cclock; + mach_timespec_t mts; + + host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock); + clock_get_time(cclock, &mts); + mach_port_deallocate(mach_task_self(), cclock); + + return (mts.tv_sec * 1000.0) + (mts.tv_nsec / 1000000.0) - _clockStart; + +#else +#error "Unknown System" +#endif + } }; -Stopwatch::Stopwatch() : - _pi(new Impl()) -{ -} +Stopwatch::Stopwatch() : _pi(new Impl()) {} -Stopwatch::~Stopwatch() -{ - delete _pi; -} +Stopwatch::~Stopwatch() { delete _pi; } -void Stopwatch::reset() -{ - _pi->reset(); -} +void Stopwatch::reset() { _pi->reset(); } -float Stopwatch::elapsedMs() -{ - return _pi->elapsedMs(); -} +float Stopwatch::elapsedMs() { return _pi->elapsedMs(); } -} -} +} // namespace xplat +} // namespace vn From e624ea124b2dc034512133e192b09b9d258c522a Mon Sep 17 00:00:00 2001 From: Bernd Pfrommer Date: Mon, 17 Jun 2024 15:07:48 -0400 Subject: [PATCH 2/5] reformatted cpp files to pass clang-format test --- vectornav/src/vectornav.cc | 14 +- vectornav/src/vectornav.hpp | 227 ++++++++++++++++--------------- vectornav/src/vn_sensor_msgs.cc | 5 +- vectornav/src/vn_sensor_msgs.hpp | 10 +- 4 files changed, 130 insertions(+), 126 deletions(-) diff --git a/vectornav/src/vectornav.cc b/vectornav/src/vectornav.cc index e39bae7e..76fa8ce4 100644 --- a/vectornav/src/vectornav.cc +++ b/vectornav/src/vectornav.cc @@ -30,7 +30,8 @@ using namespace std::chrono_literals; using namespace std::placeholders; -// Assure that the serial port is set to async low latency in order to reduce delays and package pilup. +// Assure that the serial port is set to async low latency in order +// to reduce delays and package pilup. // These changes will stay effective until the device is unplugged namespace vectornav @@ -253,7 +254,8 @@ rclcpp_action::GoalResponse Vectornav::handle_cal_goal( return rclcpp_action::GoalResponse::REJECT; } -rclcpp_action::CancelResponse Vectornav::handle_cal_cancel(const std::shared_ptr goal_handle) +rclcpp_action::CancelResponse Vectornav::handle_cal_cancel( + const std::shared_ptr goal_handle) { RCLCPP_INFO(get_logger(), "Recieved request to stop magnetic calibration"); @@ -417,7 +419,7 @@ void Vectornav::execute_cal(const std::shared_ptr goal_handle) loopRate.sleep(); } - //if we exited normally and are not cancelling + // if we exited normally and are not cancelling if (!goal_handle->is_canceling()) { // turn HSI mode to off to stop sampling // turn HSI output to enabled @@ -431,9 +433,11 @@ void Vectornav::execute_cal(const std::shared_ptr goal_handle) // reconfigure IMU but do not save // attempt reconfiguration of the device - // configure_sensor(); // ONLY FOR GDB DEBUGGING! THE BLOCK BELOW WILL HAVE NO EFFECT IF THIS IS LEFT + // re-instate the following line ONLY FOR GDB DEBUGGING! + // THE BLOCK BELOW WILL HAVE NO EFFECT IF THIS IS LEFT + // configure_sensor(); try { - // TODO Figure out why this will fail when called a second time + // TODO(Unknown) Figure out why this will fail when called a second time configure_sensor(); } catch (const std::exception & e) { RCLCPP_FATAL_STREAM( diff --git a/vectornav/src/vectornav.hpp b/vectornav/src/vectornav.hpp index ae168c75..ba2c620b 100644 --- a/vectornav/src/vectornav.hpp +++ b/vectornav/src/vectornav.hpp @@ -7,14 +7,15 @@ * https://opensource.org/licenses/MIT. */ -#ifndef VECTORNAV__VECTORNAV_HPP_ -#define VECTORNAV__VECTORNAV_HPP_ +#ifndef VECTORNAV_HPP_ +#define VECTORNAV_HPP_ #include #include #include // ROS2 +#include #include #include #include @@ -23,124 +24,124 @@ #include #include #include + #include "vectornav_msgs/action/mag_cal.hpp" -#include // VectorNav libvncxx #include "vn/compositedata.h" #include "vn/sensors.h" -namespace vectornav { - class Vectornav : public rclcpp::Node - { - public: - using MagCal = vectornav_msgs::action::MagCal; - using MagCalGH = rclcpp_action::ServerGoalHandle; - - explicit Vectornav(const rclcpp::NodeOptions & options); - ~Vectornav(); - - private: - bool optimize_serial_communication(const std::string & portName); - void reconnect_timer(); - bool configure_sensor(); - bool connect(const std::string port, const int baud); - rclcpp::Time getTimeStamp(vn::sensors::CompositeData & data); - static void ErrorPacketReceivedHandler( - void * nodeptr, vn::protocol::uart::Packet & errorPacket, size_t packetStartRunningIndex); - static void AsyncPacketReceivedHandler( - void * nodeptr, vn::protocol::uart::Packet & asyncPacket, size_t packetStartRunningIndex); - void handle_cal_accept(const std::shared_ptr goal_handle); - rclcpp_action::GoalResponse handle_cal_goal( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); - rclcpp_action::CancelResponse handle_cal_cancel(const std::shared_ptr goal_handle); - void execute_cal(const std::shared_ptr goal_handle); - /** +namespace vectornav +{ +class Vectornav : public rclcpp::Node +{ +public: + using MagCal = vectornav_msgs::action::MagCal; + using MagCalGH = rclcpp_action::ServerGoalHandle; + + explicit Vectornav(const rclcpp::NodeOptions & options); + ~Vectornav(); + +private: + bool optimize_serial_communication(const std::string & portName); + void reconnect_timer(); + bool configure_sensor(); + bool connect(const std::string port, const int baud); + rclcpp::Time getTimeStamp(vn::sensors::CompositeData & data); + static void ErrorPacketReceivedHandler( + void * nodeptr, vn::protocol::uart::Packet & errorPacket, size_t packetStartRunningIndex); + static void AsyncPacketReceivedHandler( + void * nodeptr, vn::protocol::uart::Packet & asyncPacket, size_t packetStartRunningIndex); + void handle_cal_accept(const std::shared_ptr goal_handle); + rclcpp_action::GoalResponse handle_cal_goal( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + rclcpp_action::CancelResponse handle_cal_cancel(const std::shared_ptr goal_handle); + void execute_cal(const std::shared_ptr goal_handle); + /** * Callback to take twist message and pass it to VN as velocity aiding * * \param msg Shared pointer to ROS2 geometry_msgs/Twist message containing velocity information */ - void vel_aiding_cb(const geometry_msgs::msg::Twist::SharedPtr msg); - - // - // Parsing functions - // - static void parseCommonGroup( - Vectornav * node, vn::sensors::CompositeData & compositeData, uint16_t groupFields, - const rclcpp::Time & timestamp); - static void parseTimeGroup( - Vectornav * node, vn::sensors::CompositeData & compositeData, uint16_t groupFields, - const rclcpp::Time & timestamp); - static void parseImuGroup( - Vectornav * node, vn::sensors::CompositeData & compositeData, uint16_t groupFields, - const rclcpp::Time & timestamp); - static void parseGpsGroup( - Vectornav * node, vn::sensors::CompositeData & compositeData, uint16_t groupFields, - const rclcpp::Time & timestamp); - static void parseAttitudeGroup( - Vectornav * node, vn::sensors::CompositeData & compositeData, uint16_t groupFields, - const rclcpp::Time & timestamp); - static void parseInsGroup( - Vectornav * node, vn::sensors::CompositeData & compositeData, uint16_t groupFields, - const rclcpp::Time & timestamp); - static void parseGps2Group( - Vectornav * node, vn::sensors::CompositeData & compositeData, uint16_t groupFields, - const rclcpp::Time & timestamp); - // - // Helper Functions - // - static inline geometry_msgs::msg::Vector3 toMsg(const vn::math::vec3f & rhs); - static inline geometry_msgs::msg::Quaternion toMsg(const vn::math::vec4f & rhs); - static inline geometry_msgs::msg::Point toMsg(const vn::math::vec3d & rhs); - static inline vectornav_msgs::msg::TimeUTC toMsg(const vn::protocol::uart::TimeUtc & rhs); - static inline vectornav_msgs::msg::DOP toMsg(const vn::protocol::uart::GnssDop & rhs); - static inline vectornav_msgs::msg::VpeStatus toMsg(const vn::protocol::uart::VpeStatus & rhs); - static inline std::array toMsg(const vn::math::mat3f & rhs); - static inline vectornav_msgs::msg::TimeStatus toMsg(const uint8_t rhs); - static inline vectornav_msgs::msg::InsStatus toMsg(const vn::protocol::uart::InsStatus & rhs); - - /// Count the number of set bits in a number - template - static uint countSetBits(T n) - { - T count = 0; - while (n != 0) { - n = n & (n - 1); - count++; - } - return count; - } - - // - // Member Variables - // - - /// VectorNav Sensor Handle - std::shared_ptr vs_; - - /// Reconnection Timer - rclcpp::TimerBase::SharedPtr reconnect_timer_; - - /// Publishers - rclcpp::Publisher::SharedPtr pub_common_; - rclcpp::Publisher::SharedPtr pub_time_; - rclcpp::Publisher::SharedPtr pub_imu_; - rclcpp::Publisher::SharedPtr pub_gps_; - rclcpp::Publisher::SharedPtr pub_attitude_; - rclcpp::Publisher::SharedPtr pub_ins_; - rclcpp::Publisher::SharedPtr pub_gps2_; - - /// ROS header time stamp adjustments - double averageTimeDifference_{0}; - bool adjustROSTimeStamp_{false}; - - // Subscriptions - rclcpp::Subscription::SharedPtr sub_vel_aiding_; - - /// Action servers for calibration - rclcpp_action::Server::SharedPtr server_mag_cal_; - std::thread action_thread_; - - }; -} -#endif // VECTORNAV__VECTORNAV_HPP_ + void vel_aiding_cb(const geometry_msgs::msg::Twist::SharedPtr msg); + + // + // Parsing functions + // + static void parseCommonGroup( + Vectornav * node, vn::sensors::CompositeData & compositeData, uint16_t groupFields, + const rclcpp::Time & timestamp); + static void parseTimeGroup( + Vectornav * node, vn::sensors::CompositeData & compositeData, uint16_t groupFields, + const rclcpp::Time & timestamp); + static void parseImuGroup( + Vectornav * node, vn::sensors::CompositeData & compositeData, uint16_t groupFields, + const rclcpp::Time & timestamp); + static void parseGpsGroup( + Vectornav * node, vn::sensors::CompositeData & compositeData, uint16_t groupFields, + const rclcpp::Time & timestamp); + static void parseAttitudeGroup( + Vectornav * node, vn::sensors::CompositeData & compositeData, uint16_t groupFields, + const rclcpp::Time & timestamp); + static void parseInsGroup( + Vectornav * node, vn::sensors::CompositeData & compositeData, uint16_t groupFields, + const rclcpp::Time & timestamp); + static void parseGps2Group( + Vectornav * node, vn::sensors::CompositeData & compositeData, uint16_t groupFields, + const rclcpp::Time & timestamp); + // + // Helper Functions + // + static inline geometry_msgs::msg::Vector3 toMsg(const vn::math::vec3f & rhs); + static inline geometry_msgs::msg::Quaternion toMsg(const vn::math::vec4f & rhs); + static inline geometry_msgs::msg::Point toMsg(const vn::math::vec3d & rhs); + static inline vectornav_msgs::msg::TimeUTC toMsg(const vn::protocol::uart::TimeUtc & rhs); + static inline vectornav_msgs::msg::DOP toMsg(const vn::protocol::uart::GnssDop & rhs); + static inline vectornav_msgs::msg::VpeStatus toMsg(const vn::protocol::uart::VpeStatus & rhs); + static inline std::array toMsg(const vn::math::mat3f & rhs); + static inline vectornav_msgs::msg::TimeStatus toMsg(const uint8_t rhs); + static inline vectornav_msgs::msg::InsStatus toMsg(const vn::protocol::uart::InsStatus & rhs); + + /// Count the number of set bits in a number + template + static uint countSetBits(T n) + { + T count = 0; + while (n != 0) { + n = n & (n - 1); + count++; + } + return count; + } + + // + // Member Variables + // + + /// VectorNav Sensor Handle + std::shared_ptr vs_; + + /// Reconnection Timer + rclcpp::TimerBase::SharedPtr reconnect_timer_; + + /// Publishers + rclcpp::Publisher::SharedPtr pub_common_; + rclcpp::Publisher::SharedPtr pub_time_; + rclcpp::Publisher::SharedPtr pub_imu_; + rclcpp::Publisher::SharedPtr pub_gps_; + rclcpp::Publisher::SharedPtr pub_attitude_; + rclcpp::Publisher::SharedPtr pub_ins_; + rclcpp::Publisher::SharedPtr pub_gps2_; + + /// ROS header time stamp adjustments + double averageTimeDifference_{0}; + bool adjustROSTimeStamp_{false}; + + // Subscriptions + rclcpp::Subscription::SharedPtr sub_vel_aiding_; + + /// Action servers for calibration + rclcpp_action::Server::SharedPtr server_mag_cal_; + std::thread action_thread_; +}; +} // namespace vectornav +#endif // VECTORNAV_HPP_ diff --git a/vectornav/src/vn_sensor_msgs.cc b/vectornav/src/vn_sensor_msgs.cc index bcfd1e3f..8f7f7c02 100644 --- a/vectornav/src/vn_sensor_msgs.cc +++ b/vectornav/src/vn_sensor_msgs.cc @@ -90,7 +90,7 @@ VnSensorMsgs::VnSensorMsgs(const rclcpp::NodeOptions & options) : Node("vn_senso sub_vn_gps2_ = this->create_subscription( "vectornav/raw/gps2", 10, sub_vn_gps2_cb); - //enu frame option + // enu frame option use_enu = get_parameter("use_enu").as_bool(); } @@ -404,7 +404,6 @@ void VnSensorMsgs::fill_covariance_from_param( length); } } -} - +} // namespace vectornav RCLCPP_COMPONENTS_REGISTER_NODE(vectornav::VnSensorMsgs) diff --git a/vectornav/src/vn_sensor_msgs.hpp b/vectornav/src/vn_sensor_msgs.hpp index 2c32a5de..8511082a 100644 --- a/vectornav/src/vn_sensor_msgs.hpp +++ b/vectornav/src/vn_sensor_msgs.hpp @@ -7,8 +7,8 @@ * https://opensource.org/licenses/MIT. */ -#ifndef VECTORNAV__VN_SENSOR_MSGS_HPP_ -#define VECTORNAV__VN_SENSOR_MSGS_HPP_ +#ifndef VN_SENSOR_MSGS_HPP_ +#define VN_SENSOR_MSGS_HPP_ #include #include #include @@ -35,7 +35,7 @@ namespace vectornav class VnSensorMsgs : public rclcpp::Node { public: - VnSensorMsgs(const rclcpp::NodeOptions & options); + explicit VnSensorMsgs(const rclcpp::NodeOptions & options); private: void sub_vn_common(const vectornav_msgs::msg::CommonGroup::SharedPtr msg_in) const; @@ -47,7 +47,7 @@ class VnSensorMsgs : public rclcpp::Node void sub_vn_gps2(const vectornav_msgs::msg::GpsGroup::SharedPtr msg_in) const; void fill_covariance_from_param(std::string param_name, std::array & array) const; /// Convert from DEG to RAD -inline static double deg2rad(double in) { return in * M_PI / 180.0; } + inline static double deg2rad(double in) { return in * M_PI / 180.0; } // // Member Variables @@ -103,4 +103,4 @@ inline static double deg2rad(double in) { return in * M_PI / 180.0; } geometry_msgs::msg::Point ins_posecef_; }; } // namespace vectornav -#endif // VECTORNAV__VN_SENSOR_MSGS_HPP_ +#endif // VN_SENSOR_MSGS_HPP_ From 8d12ba690c5e1ccb2483d24fac9e82ed17213966 Mon Sep 17 00:00:00 2001 From: Bernd Pfrommer Date: Mon, 17 Jun 2024 15:08:33 -0400 Subject: [PATCH 3/5] reformat python files to pass flake8 --- vectornav/launch/vectornav.launch.py | 17 +++++----- .../launch/vectornav_composable.launch.py | 34 +++++++++++-------- 2 files changed, 28 insertions(+), 23 deletions(-) diff --git a/vectornav/launch/vectornav.launch.py b/vectornav/launch/vectornav.launch.py index ba1392d8..0c08e02c 100644 --- a/vectornav/launch/vectornav.launch.py +++ b/vectornav/launch/vectornav.launch.py @@ -1,26 +1,27 @@ import os from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription from launch_ros.actions import Node -def generate_launch_description(): +def generate_launch_description(): this_dir = get_package_share_directory('vectornav') - + # Vectornav start_vectornav_cmd = Node( - package='vectornav', + package='vectornav', executable='vectornav', output='screen', - parameters=[os.path.join(this_dir, 'config', 'vectornav.yaml')]) - + parameters=[os.path.join(this_dir, 'config', 'vectornav.yaml')], + ) + start_vectornav_sensor_msgs_cmd = Node( - package='vectornav', + package='vectornav', executable='vn_sensor_msgs', output='screen', - parameters=[os.path.join(this_dir, 'config', 'vectornav.yaml')]) + parameters=[os.path.join(this_dir, 'config', 'vectornav.yaml')], + ) # Create the launch description and populate ld = LaunchDescription() diff --git a/vectornav/launch/vectornav_composable.launch.py b/vectornav/launch/vectornav_composable.launch.py index 5b5c37ab..93298d58 100644 --- a/vectornav/launch/vectornav_composable.launch.py +++ b/vectornav/launch/vectornav_composable.launch.py @@ -1,16 +1,14 @@ - # # launch file for composable node container # from launch import LaunchDescription -from launch_ros.substitutions import FindPackageShare from launch.substitutions import PathJoinSubstitution from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - con = ComposableNodeContainer( name='vectornav_container', namespace='', @@ -21,22 +19,28 @@ def generate_launch_description(): package='vectornav', plugin='vectornav::Vectornav', name='vectornav', - parameters=[PathJoinSubstitution( - [FindPackageShare('vectornav'), - 'config', 'vectornav_composable.yaml'])], + parameters=[ + PathJoinSubstitution( + [FindPackageShare('vectornav'), 'config', 'vectornav_composable.yaml'] + ) + ], remappings=[], - extra_arguments=[{'use_intra_process_comms': True}]), + extra_arguments=[{'use_intra_process_comms': True}], + ), ComposableNode( package='vectornav', plugin='vectornav::VnSensorMsgs', name='vn_sensor_msgs', - parameters=[PathJoinSubstitution( - [FindPackageShare('vectornav'), - 'config', 'vn_sensor_msgs_composable.yaml'])], + parameters=[ + PathJoinSubstitution( + [FindPackageShare('vectornav'), 'config', 'vn_sensor_msgs_composable.yaml'] + ) + ], remappings=[], - extra_arguments=[{'use_intra_process_comms': True}]) - ], - output='screen') - - return LaunchDescription([con]) + extra_arguments=[{'use_intra_process_comms': True}], + ), + ], + output='screen', + ) + return LaunchDescription([con]) From 9b6e90fccc459185f623bdfcf148fe99a4d9ee0a Mon Sep 17 00:00:00 2001 From: Bernd Pfrommer Date: Mon, 17 Jun 2024 15:09:48 -0400 Subject: [PATCH 4/5] enable linter tests --- vectornav/.clang-format | 1 + vectornav/CMakeLists.txt | 24 ++++++++++++++++++++---- vectornav/CPPLINT.cfg | 2 ++ vectornav/package.xml | 1 + vectornav_msgs/CMakeLists.txt | 4 ++-- 5 files changed, 26 insertions(+), 6 deletions(-) create mode 120000 vectornav/.clang-format create mode 100644 vectornav/CPPLINT.cfg diff --git a/vectornav/.clang-format b/vectornav/.clang-format new file mode 120000 index 00000000..7cab60ce --- /dev/null +++ b/vectornav/.clang-format @@ -0,0 +1 @@ +../.clang-format \ No newline at end of file diff --git a/vectornav/CMakeLists.txt b/vectornav/CMakeLists.txt index 48a4dcb5..e1b6a1c2 100644 --- a/vectornav/CMakeLists.txt +++ b/vectornav/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.16) project(vectornav) # Default to C99 @@ -73,9 +73,25 @@ install(DIRECTORY ) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() - list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + find_package(ament_cmake REQUIRED) +# find_package(ament_cmake_copyright REQUIRED) + find_package(ament_cmake_cppcheck REQUIRED) + find_package(ament_cmake_cpplint REQUIRED) + find_package(ament_cmake_clang_format REQUIRED) + find_package(ament_cmake_flake8 REQUIRED) + find_package(ament_cmake_lint_cmake REQUIRED) + find_package(ament_cmake_pep257 REQUIRED) + find_package(ament_cmake_xmllint REQUIRED) + +# ament_copyright() + ament_cppcheck(LANGUAGE c++) + ament_cpplint(FILTERS "-build/include,-runtime/indentation_namespace") + ament_clang_format(CONFIG_FILE .clang-format) + ament_flake8() + ament_lint_cmake() + ament_pep257() + ament_xmllint() + endif() ament_export_dependencies(rosidl_default_runtime) diff --git a/vectornav/CPPLINT.cfg b/vectornav/CPPLINT.cfg new file mode 100644 index 00000000..c858fbd5 --- /dev/null +++ b/vectornav/CPPLINT.cfg @@ -0,0 +1,2 @@ +# which files to exclude +exclude_files=vnproglib-1.2.0.0 diff --git a/vectornav/package.xml b/vectornav/package.xml index 55930db9..8089d455 100644 --- a/vectornav/package.xml +++ b/vectornav/package.xml @@ -22,6 +22,7 @@ ament_lint_auto ament_lint_common + ament_cmake_clang_format ament_cmake diff --git a/vectornav_msgs/CMakeLists.txt b/vectornav_msgs/CMakeLists.txt index a822b25d..c2f2c04b 100644 --- a/vectornav_msgs/CMakeLists.txt +++ b/vectornav_msgs/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.16) project(vectornav_msgs) # Default to C99 @@ -35,4 +35,4 @@ rosidl_generate_interfaces(${PROJECT_NAME} ament_export_dependencies(rosidl_default_runtime) -ament_package() \ No newline at end of file +ament_package() From e45f5dfa572d29e554139eb75547f0255d74c564 Mon Sep 17 00:00:00 2001 From: Bernd Pfrommer Date: Mon, 17 Jun 2024 15:10:09 -0400 Subject: [PATCH 5/5] enable CI workflow --- .github/workflows/ci.yaml | 68 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 68 insertions(+) create mode 100644 .github/workflows/ci.yaml diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml new file mode 100644 index 00000000..0035e519 --- /dev/null +++ b/.github/workflows/ci.yaml @@ -0,0 +1,68 @@ +# +# continuous integration workflow +# +name: build repo + +on: + push: + branches: [ros2] + pull_request: + branches: [ros2] + workflow_dispatch: + branches: [ros2] + +jobs: + build_on_jammy: + name: build_jammy_${{ matrix.distro }} + runs-on: ubuntu-22.04 + strategy: + fail-fast: false + matrix: + distro: [humble, iron] + steps: + - uses: actions/checkout@v3 + with: + path: src/${{ github.event.repository.name }} + fetch-depth: 0 + - uses: ros-tooling/setup-ros@0.7.4 + with: + required-ros-distributions: ${{ matrix.distro }} + - uses: ros-tooling/action-ros-ci@0.3.11 + with: + target-ros2-distro: ${{ matrix.distro }} + colcon-defaults: | + { + "build": { + "cmake-args": [ + "-DCMAKE_BUILD_TYPE=RelWithDebInfo" + ] + } + } + build_on_noble: + name: build_noble_${{ matrix.distro }} + runs-on: ubuntu-latest + container: + image: ubuntu:noble + strategy: + fail-fast: false + matrix: + distro: [jazzy, rolling] + steps: + - uses: actions/checkout@v3 + with: + path: src/${{ github.event.repository.name }} + fetch-depth: 0 + - uses: ros-tooling/setup-ros@0.7.4 + with: + required-ros-distributions: ${{ matrix.distro }} + - uses: ros-tooling/action-ros-ci@0.3.11 + with: + target-ros2-distro: ${{ matrix.distro }} + colcon-defaults: | + { + "build": { + "cmake-args": [ + "-DCMAKE_BUILD_TYPE=RelWithDebInfo" + ] + } + }