This commit is contained in:
r.koeppe
2024-05-14 02:14:13 +02:00
parent 0052d3984b
commit 2d22ccd2d6
1423 changed files with 354055 additions and 7 deletions

View File

@ -0,0 +1,86 @@
#ifndef _VNMATH_ATTITUDE_H_
#define _VNMATH_ATTITUDE_H_
#include "vector.h"
#include "matrix.h"
#include "export.h"
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
};
public:
/// \brief Returns an <c>AttitudeF</c> representing no rotation.
static AttitudeF noRotation();
/// \brief Creates a new <c>AttitudeF</c> from a quaternion.
///
/// \param[in] quat The orientation expressed as a quaternion.
/// \return The new <c>AttitudeF</c>.
static AttitudeF fromQuat(vec4f quat);
/// \brief Creates a new <c>AttitudeF</c> from a yaw, pitch, roll in degrees.
///
/// \param[in] yprInDegs The yaw, pitch, roll in degrees.
/// \return The new <c>AttitudeF</c>.
static AttitudeF fromYprInDegs(vec3f yprInDegs);
/// \brief Creates a new <c>AttitudeF</c> from a yaw, pitch, roll in radians.
///
/// \param[in] yprInRads The yaw, pitch, roll in radians.
/// \return The new <c>AttitudeF</c>.
static AttitudeF fromYprInRads(vec3f yprInRads);
/// \brief Creates a new <c>AttitudeF</c> from a direction cosine matrix.
///
/// \param[in] dcm The direction cosine matrix.
/// \return The new <c>AttitudeF</c>.
static AttitudeF fromDcm(mat3f dcm);
// TEMP
AttitudeF() { }
private:
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 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 by a direction cosine matrix.
/// \return The orientation as a direction cosine matrix.
mat3f dcm();
private:
AttitudeType _underlyingType;
uint8_t _data[sizeof(mat3f)];
};
}
}
#endif

View File

@ -0,0 +1,23 @@
#ifndef _VN_UTIL_BOOSTPYTHON_H
#define _VN_UTIL_BOOSTPYTHON_H
// 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)
#endif
#include "boost/python.hpp"
#if defined (_MSC_VER)
#pragma warning(pop)
#endif
#endif

View File

@ -0,0 +1,79 @@
#ifndef _VN_UTIL_COMPILER_H
#define _VN_UTIL_COMPILER_H
// This header provides some simple checks for various features supported by the
// current compiler.
// The VN_HAS_RANGE_LOOP define indicates if the compiler supports range based
// loops.
//
// for (auto i : _items)
// cout << i << endl;
//
// HACK: Currently not checking the compiler and defaulting to no range loop support.
#define VN_HAS_RANGE_LOOP 0
// The VN_SUPPORTS_SWAP define indicates if the compiler supports the swap
// operation for STL items.
//
// [Example]
//
// #if VN_SUPPORTS_SWAP
// queue<Packet> empty;
// _receivedResponses.swap(empty);
// #else
// while (!_receivedResponses.empty()) _receivedResponses.pop();
// #endif
//
#if (defined(_MSC_VER) && _MSC_VER > 1500) || (__cplusplus >= 201103L)
#define VN_SUPPORTS_SWAP 1
#else
#define VN_SUPPORTS_SWAP 0
#endif
// The VN_SUPPORTS_INITIALIZER_LIST define indicates if the compiler supports
// using an initialization list to initialize STL containers.
//
// [Example]
//
// #if VN_SUPPORTS_INITIALIZER_LIST
// vector<CompositeData*> d({ &ez->_persistentData, &nd });
// #else
// vector<CompositeData*> d(2);
// d[0] = &ez->_persistentData;
// d[1] = &nd;
// #endif
//
#if (defined(_MSC_VER) && _MSC_VER <= 1600) || (__cplusplus < 201103L)
#define VN_SUPPORTS_INITIALIZER_LIST 0
#else
#define VN_SUPPORTS_INITIALIZER_LIST 1
#endif
// The VN_SUPPORTS_CSTR_STRING_CONCATENATE define indictes if the compiler supports
// concatenating a C-style string with std::string using the '+' operator.
//
// [Example]
//
// #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
//
#if (defined(_MSC_VER) && _MSC_VER <= 1600)
#define VN_SUPPORTS_CSTR_STRING_CONCATENATE 0
#else
#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
#else
#define VN_HAVE_SECURE_CRT 0
#define VN_HAVE_SECURE_SCL 0
#endif
#endif

View File

@ -0,0 +1,816 @@
#ifndef _VNSENSORS_COMPOSITEDATA_H_
#define _VNSENSORS_COMPOSITEDATA_H_
#include <vector>
#include "vn/export.h"
#include "vn/packet.h"
#include "vn/attitude.h"
#include "vn/position.h"
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<CompositeData*>& o);
/// \brief Resets the data contained in the CompositeData object.
void reset();
/// \brief Indicates if <c>anyAttitude</c> has valid data.
/// \return <c>true</c> if <c>anyAttitude</c> has valid data; otherwise <c>false</c>.
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 <c>yawPitchRoll</c> has valid data.
/// \return <c>true</c> if <c>yawPitchRoll</c> has valid data; otherwise <c>false</c>.
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 <c>quaternion</c> has valid data.
/// \return <c>true</c> if <c>quaternion</c> has valid data; otherwise <c>false</c>.
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 <c>directionCosineMatrix</c> has valid data.
/// \return <c>true</c> if <c>directionCosineMatrix</c> has valid data; otherwise <c>false</c>.
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 <c>anyMagnetic</c> has valid data.
/// \return <c>true</c> if <c>anyMagnetic</c> has valid data; otherwise <c>false</c>.
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 <c>magnetic</c> has valid data.
/// \return <c>true</c> if <c>magnetic</c> has valid data; otherwise <c>false</c>.
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 <c>magneticUncompensated</c> has valid data.
/// \return <c>true</c> if <c>magneticUncompensated</c> has valid data; otherwise <c>false</c>.
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 <c>magneticNed</c> has valid data.
/// \return <c>true</c> if <c>magneticNed</c> has valid data; otherwise <c>false</c>.
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 <c>magneticEcef</c> has valid data.
/// \return <c>true</c> if <c>magneticEcef</c> has valid data; otherwise <c>false</c>.
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 <c>anyAcceleration</c> has valid data.
/// \return <c>true</c> if <c>anyAcceleration</c> has valid data; otherwise <c>false</c>.
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 <c>acceleration</c> has valid data.
/// \return <c>true</c> if <c>acceleration</c> has valid data; otherwise <c>false</c>.
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 <c>accelerationLinearBody</c> has valid data.
/// \return <c>true</c> if <c>accelerationLinearBody</c> has valid data; otherwise <c>false</c>.
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 <c>accelerationUncompensated</c> has valid data.
/// \return <c>true</c> if <c>accelerationUncompensated</c> has valid data; otherwise <c>false</c>.
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 <c>accelerationLinearNed</c> has valid data.
/// \return <c>true</c> if <c>accelerationLinearNed</c> has valid data; otherwise <c>false</c>.
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 <c>accelerationLinearEcef</c> has valid data.
/// \return <c>true</c> if <c>accelerationLinearEcef</c> has valid data; otherwise <c>false</c>.
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 <c>accelerationNed</c> has valid data.
/// \return <c>true</c> if <c>accelerationNed</c> has valid data; otherwise <c>false</c>.
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 <c>accelerationEcef</c> has valid data.
/// \return <c>true</c> if <c>accelerationEcef</c> has valid data; otherwise <c>false</c>.
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 <c>anyAngularRate</c> has valid data.
/// \return <c>true</c> if <c>anyAngularRate</c> has valid data; otherwise <c>false</c>.
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 <c>angularRate</c> has valid data.
/// \return <c>true</c> if <c>angularRate</c> has valid data; otherwise <c>false</c>.
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 <c>angularRateUncompensated</c> has valid data.
/// \return <c>true</c> if <c>angularRateUncompensated</c> has valid data; otherwise <c>false</c>.
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 <c>anyTemperature</c> has valid data.
/// \return <c>true</c> if <c>anyTemperature</c> has valid data; otherwise <c>false</c>.
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 <c>temperature</c> has valid data.
/// \return <c>true</c> if <c>temperature</c> has valid data; otherwise <c>false</c>.
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 <c>anyPressure</c> has valid data.
/// \return <c>true</c> if <c>anyPressure</c> has valid data; otherwise <c>false</c>.
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 <c>pressure</c> has valid data.
/// \return <c>true</c> if <c>pressure</c> has valid data; otherwise <c>false</c>.
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 <c>anyPosition</c> has valid data.
/// \return <c>true</c> if <c>anyPosition</c> has valid data; otherwise <c>false</c>.
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 <c>positionGpsLla</c> has valid data.
/// \return <c>true</c> if <c>positionGpsLla</c> has valid data; otherwise <c>false</c>.
bool hasPositionGpsLla();
/// \brief Indicates if <c>positionGps2Lla</c> has valid data.
/// \return <c>true</c> if <c>positionGps2Lla</c> has valid data; otherwise <c>false</c>.
bool hasPositionGps2Lla();
/// \brief Position GPS LLA data.
/// \return The Position GPS LLA data.
/// \exception invalid_operation Thrown if there is not any valid data.
math::vec3d positionGpsLla();
/// \brief Position GPS2 LLA data.
/// \return The Position GPS2 LLA data.
/// \exception invalid_operation Thrown if there is not any valid data.
math::vec3d positionGps2Lla();
/// \brief Indicates if <c>positionGpsEcef</c> has valid data.
/// \return <c>true</c> if <c>positionGpsEcef</c> has valid data; otherwise <c>false</c>.
bool hasPositionGpsEcef();
/// \brief Indicates if <c>positionGps2Ecef</c> has valid data.
/// \return <c>true</c> if <c>positionGps2Ecef</c> has valid data; otherwise <c>false</c>.
bool hasPositionGps2Ecef();
/// \brief Position GPS ECEF data.
/// \return The Position GPS ECEF data.
/// \exception invalid_operation Thrown if there is not any valid data.
math::vec3d positionGpsEcef();
/// \brief Position GPS2 ECEF data.
/// \return The Position GPS2 ECEF data.
/// \exception invalid_operation Thrown if there is not any valid data.
math::vec3d positionGps2Ecef();
/// \brief Indicates if <c>positionEstimatedLla</c> has valid data.
/// \return <c>true</c> if <c>positionEstimatedLla</c> has valid data; otherwise <c>false</c>.
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 <c>positionEstimatedEcef</c> has valid data.
/// \return <c>true</c> if <c>positionEstimatedEcef</c> has valid data; otherwise <c>false</c>.
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 <c>anyVelocity</c> has valid data.
/// \return <c>true</c> if <c>anyVelocity</c> has valid data; otherwise <c>false</c>.
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 <c>velocityGpsNed</c> has valid data.
/// \return <c>true</c> if <c>velocityGpsNed</c> has valid data; otherwise <c>false</c>.
bool hasVelocityGpsNed();
/// \brief Indicates if <c>velocityGps2Ned</c> has valid data.
/// \return <c>true</c> if <c>velocityGps2Ned</c> has valid data; otherwise <c>false</c>.
bool hasVelocityGps2Ned();
/// \brief Velocity GPS NED data.
/// \return The velocity GPS NED data.
/// \exception invalid_operation Thrown if there is not any valid data.
math::vec3f velocityGpsNed();
/// \brief Velocity GPS2 NED data.
/// \return The velocity GPS2 NED data.
/// \exception invalid_operation Thrown if there is not any valid data.
math::vec3f velocityGps2Ned();
/// \brief Indicates if <c>velocityGpsEcef</c> has valid data.
/// \return <c>true</c> if <c>velocityGpsEcef</c> has valid data; otherwise <c>false</c>.
bool hasVelocityGpsEcef();
/// \brief Indicates if <c>velocityGps2Ecef</c> has valid data.
/// \return <c>true</c> if <c>velocityGps2Ecef</c> has valid data; otherwise <c>false</c>.
bool hasVelocityGps2Ecef();
/// \brief Velocity GPS ECEF data.
/// \return The velocity GPS ECEF data.
/// \exception invalid_operation Thrown if there is not any valid data.
math::vec3f velocityGpsEcef();
/// \brief Velocity GPS2 ECEF data.
/// \return The velocity GPS2 ECEF data.
/// \exception invalid_operation Thrown if there is not any valid data.
math::vec3f velocityGps2Ecef();
/// \brief Indicates if <c>velocityEstimatedNed</c> has valid data.
/// \return <c>true</c> if <c>velocityEstimatedNed</c> has valid data; otherwise <c>false</c>.
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 <c>velocityEstimatedEcef</c> has valid data.
/// \return <c>true</c> if <c>velocityEstimatedEcef</c> has valid data; otherwise <c>false</c>.
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 <c>velocityEstimatedBody</c> has valid data.
/// \return <c>true</c> if <c>velocityEstimatedBody</c> has valid data; otherwise <c>false</c>.
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 <c>deltaTime</c> has valid data.
/// \return <c>true</c> if <c>deltaTime</c> has valid data; otherwise <c>false</c>.
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 <c>deltaTheta</c> has valid data.
/// \return <c>true</c> if <c>deltaTheta</c> has valid data; otherwise <c>false</c>.
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 <c>deltaVelocity</c> has valid data.
/// \return <c>true</c> if <c>deltaVelocity</c> has valid data; otherwise <c>false</c>.
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 <c>timeStartup</c> has valid data.
/// \return <c>true</c> if <c>timeStartup</c> has valid data; otherwise <c>false</c>.
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 <c>timeGps</c> has valid data.
/// \return <c>true</c> if <c>timeGps</c> has valid data; otherwise <c>false</c>.
bool hasTimeGps();
/// \brief Indicates if <c>timeGps2</c> has valid data.
/// \return <c>true</c> if <c>timeGps2</c> has valid data; otherwise <c>false</c>.
bool hasTimeGps2();
/// \brief Time GPS data.
/// \return The time GPS data.
/// \exception invalid_operation Thrown if there is not any valid data.
uint64_t timeGps();
/// \brief Time GPS2 data.
/// \return The time GPS2 data.
/// \exception invalid_operation Thrown if there is not any valid data.
uint64_t timeGps2();
/// \brief Indicates if <c>tow</c> has valid data.
/// \return <c>true</c> if <c>tow</c> has valid data; otherwise <c>false</c>.
bool hasTow();
/// \brief GPS time of week data.
/// \return The GPS time of week data.
/// \exception invalid_operation Thrown if there is not any valid data.
double tow();
/// \brief Indicates if <c>week</c> has valid data.
/// \return <c>true</c> if <c>week</c> has valid data; otherwise <c>false</c>.
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 <c>numSats</c> has valid data.
/// \return <c>true</c> if <c>numSats</c> has valid data; otherwise <c>false</c>.
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 <c>timeSyncIn</c> has valid data.
/// \return <c>true</c> if <c>timeSyncIn</c> has valid data; otherwise <c>false</c>.
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 <c>vpeStatus</c> has valid data.
/// \return <c>true</c> if <c>vpeStatus</c> has valid data; otherwise <c>false</c>.
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 <c>insStatus</c> has valid data.
/// \return <c>true</c> if <c>insStatus</c> has valid data; otherwise <c>false</c>.
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 <c>syncInCnt</c> has valid data.
/// \return <c>true</c> if <c>syncInCnt</c> has valid data; otherwise <c>false</c>.
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 <c>syncOutCnt</c> has valid data.
/// \return <c>true</c> if <c>syncOutCnt</c> has valid data; otherwise <c>false</c>.
bool hasSyncOutCnt();
/// \brief SyncOutCnt data.
/// \return The SyncOutCnt data.
/// \exception invalid_operation Thrown if there is not any valid data.
uint32_t syncOutCnt();
/// \brief Indicates if <c>timeStatus</c> has valid data.
/// \return <c>true</c> if <c>timeStatus</c> has valid data; otherwise <c>false</c>.
bool hasTimeStatus();
/// \brief TimeStatus data.
/// \return The TimeStatus data.
/// \exception invalid_operation Thrown if there is not any valid data.
uint8_t timeStatus();
/// \brief Indicates if <c>timeGpsPps</c> has valid data.
/// \return <c>true</c> if <c>timeGpsPps</c> has valid data; otherwise <c>false</c>.
bool hasTimeGpsPps();
/// \brief Indicates if <c>timeGps2Pps</c> has valid data.
/// \return <c>true</c> if <c>timeGps2Pps</c> has valid data; otherwise <c>false</c>.
bool hasTimeGps2Pps();
/// \brief TimeGpsPps data.
/// \return The TimeGpsPps data.
/// \exception invalid_operation Thrown if there is not any valid data.
uint64_t timeGpsPps();
/// \brief TimeGps2Pps data.
/// \return The TimeGps2Pps data.
/// \exception invalid_operation Thrown if there is not any valid data.
uint64_t timeGps2Pps();
/// \brief Indicates if <c>gpsTow</c> has valid data.
/// \return <c>true</c> if <c>gpsTow</c> has valid data; otherwise <c>false</c>.
bool hasGpsTow();
/// \brief Indicates if <c>gps2Tow</c> has valid data.
/// \return <c>true</c> if <c>gps2Tow</c> has valid data; otherwise <c>false</c>.
bool hasGps2Tow();
/// \brief GpsTow data.
/// \return The GpsTow data.
/// \exception invalid_operation Thrown if there is not any valid data.
uint64_t gpsTow();
/// \brief Gps2Tow data.
/// \return The Gps2Tow data.
/// \exception invalid_operation Thrown if there is not any valid data.
uint64_t gps2Tow();
/// \brief Indicates if <c>timeUtc</c> has valid data.
/// \return <c>true</c> if <c>timeUtc</c> has valid data; otherwise <c>false</c>.
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 Indicates if <c>sensSat</c> has valid data.
/// \return <c>true</c> if <c>sensSat</c> has valid data; otherwise <c>false</c>.
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 Indicates if <c>fix</c> has valid data.
/// \return <c>true</c> if <c>fix</c> has valid data; otherwise <c>false</c>.
bool hasFix();
/// \brief Indicates if <c>fix2</c> has valid data.
/// \return <c>true</c> if <c>fix2</c> has valid data; otherwise <c>false</c>.
bool hasFix2();
/// \brief GPS fix data.
/// \return The GPS fix data.
/// \exception invalid_operation Thrown if there is not any valid data.
protocol::uart::GpsFix fix();
/// \brief GPS2 fix data.
/// \return The GPS2 fix data.
/// \exception invalid_operation Thrown if there is not any valid data.
protocol::uart::GpsFix fix2();
/// \brief Indicates if <c>anyPositionUncertainty</c> has valid data.
/// \return <c>true</c> if <c>anyPositionUncertainty</c> has valid data; otherwise <c>false</c>.
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 Indicates if <c>positionUncertaintyGpsNed</c> has valid data.
/// \return <c>true</c> if <c>positionUncertaintyGpsNed</c> has valid data; otherwise <c>false</c>.
bool hasPositionUncertaintyGpsNed();
/// \brief Indicates if <c>positionUncertaintyGps2Ned</c> has valid data.
/// \return <c>true</c> if <c>positionUncertaintyGps2Ned</c> has valid data; otherwise <c>false</c>.
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();
/// \brief GPS2 position uncertainty NED data.
/// \return The GPS2 position uncertainty NED data.
/// \exception invalid_operation Thrown if there is not any valid data.
math::vec3f positionUncertaintyGps2Ned();
/// \brief Indicates if <c>positionUncertaintyGpsEcef</c> has valid data.
/// \return <c>true</c> if <c>positionUncertaintyGpsEcef</c> has valid data; otherwise <c>false</c>.
bool hasPositionUncertaintyGpsEcef();
/// \brief Indicates if <c>positionUncertaintyGps2Ecef</c> has valid data.
/// \return <c>true</c> if <c>positionUncertaintyGps2Ecef</c> has valid data; otherwise <c>false</c>.
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();
/// \brief GPS2 position uncertainty ECEF data.
/// \return The GPS2 position uncertainty ECEF data.
/// \exception invalid_operation Thrown if there is not any valid data.
math::vec3f positionUncertaintyGps2Ecef();
/// \brief Indicates if <c>positionUncertaintyEstimated</c> has valid data.
/// \return <c>true</c> if <c>positionUncertaintyEstimated</c> has valid data; otherwise <c>false</c>.
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 Indicates if <c>anyVelocityUncertainty</c> has valid data.
/// \return <c>true</c> if <c>anyVelocityUncertainty</c> has valid data; otherwise <c>false</c>.
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 Indicates if <c>velocityUncertaintyGps</c> has valid data.
/// \return <c>true</c> if <c>velocityUncertaintyGps</c> has valid data; otherwise <c>false</c>.
bool hasVelocityUncertaintyGps();
/// \brief Indicates if <c>velocityUncertaintyGps2</c> has valid data.
/// \return <c>true</c> if <c>velocityUncertaintyGps2</c> has valid data; otherwise <c>false</c>.
bool hasVelocityUncertaintyGps2();
/// \brief GPS velocity uncertainty data.
/// \return The GPS velocity uncertainty data.
/// \exception invalid_operation Thrown if there is not any valid data.
float velocityUncertaintyGps();
/// \brief GPS2 velocity uncertainty data.
/// \return The GPS2 velocity uncertainty data.
/// \exception invalid_operation Thrown if there is not any valid data.
float velocityUncertaintyGps2();
/// \brief Indicates if <c>velocityUncertaintyEstimated</c> has valid data.
/// \return <c>true</c> if <c>velocityUncertaintyEstimated</c> has valid data; otherwise <c>false</c>.
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 <c>timeUncertainty</c> has valid data.
/// \return <c>true</c> if <c>timeUncertainty</c> has valid data; otherwise <c>false</c>.
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 <c>attitudeUncertainty</c> has valid data.
/// \return <c>true</c> if <c>attitudeUncertainty</c> has valid data; otherwise <c>false</c>.
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 <c>courseOverGround</c> has valid data.
/// \return <c>true</c> if <c>courseOverGround</c> havs valid data; otherwise <c>false</c>.
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 <c>speedOverGround</c> has valid data.
/// \return <c>true</c> if <c>speedOverGround</c> havs valid data; otherwise <c>false</c>.
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 <c>timeInfo</c> has valid data.
/// \return <c>true</c> if <c>timeInfo</c> havs valid data; otherwise <c>false</c>.
bool hasTimeInfo();
/// \brief GPS Time Status and number of leap seconds.
///
/// \return Current Time Info.
/// \exception invalid_operation Thrown if there is no valid data.
protocol::uart::TimeInfo timeInfo();
/// \brief GPS2 Time Status and number of leap seconds.
///
/// \return Current Time Info.
/// \exception invalid_operation Thrown if there is no valid data.
protocol::uart::TimeInfo timeInfo2();
/// \brief Indicates if <c>dop</c> has valid data.
/// \return <c>true</c> if <c>dop</c> havs valid data; otherwise <c>false</c>.
bool hasDop();
/// \brief Dilution of precision data.
///
/// \return Current DOP values.
/// \exception invalid_operation Thrown if there is no valid data.
protocol::uart::GnssDop dop();
private:
static void parseBinary(protocol::uart::Packet& p, std::vector<CompositeData*>& o);
static void parseAscii(protocol::uart::Packet& p, std::vector<CompositeData*>& o);
static void parseBinaryPacketCommonGroup(protocol::uart::Packet& p, protocol::uart::CommonGroup gf, std::vector<CompositeData*>& o);
static void parseBinaryPacketTimeGroup(protocol::uart::Packet& p, protocol::uart::TimeGroup gf, std::vector<CompositeData*>& o);
static void parseBinaryPacketImuGroup(protocol::uart::Packet& p, protocol::uart::ImuGroup gf, std::vector<CompositeData*>& o);
static void parseBinaryPacketGpsGroup(protocol::uart::Packet& p, protocol::uart::GpsGroup gf, std::vector<CompositeData*>& o);
static void parseBinaryPacketAttitudeGroup(protocol::uart::Packet& p, protocol::uart::AttitudeGroup gf, std::vector<CompositeData*>& o);
static void parseBinaryPacketInsGroup(protocol::uart::Packet& p, protocol::uart::InsGroup gf, std::vector<CompositeData*>& o);
static void parseBinaryPacketGps2Group(protocol::uart::Packet& p, protocol::uart::GpsGroup gf, std::vector<CompositeData*>& o);
private:
struct Impl;
Impl* _i;
template<typename T>
static void setValues(T val, std::vector<CompositeData*>& o, void (Impl::* function)(T))
{
for (std::vector<CompositeData*>::iterator i = o.begin(); i != o.end(); ++i)
(*((*i)->_i).*function)(val);
}
};
}
}
#endif

View File

@ -0,0 +1,48 @@
/// \file
/// {COMMON_HEADER}
///
/// \section DESCRIPTION
/// This header files containts constants for the VectorNav C++ Math Library.
#ifndef _VN_MATH_CONSTS_H_
#define _VN_MATH_CONSTS_H_
namespace vn {
namespace math {
/// \defgroup pi_constants PI Constants
/// \brief Convenient PI constants.
/// \{
/// \brief PI in single-precision.
static const float PI = 3.141592653589793238f;
/// \brief PI in single-precision.
static const float PIf = PI;
/// \brief PI in double-precision.
static const double PId = 3.141592653589793238;
/// \brief PI * 2 in single-precision.
static const float PI2 = 6.283185307179586476f;
/// \brief PI * 2 in single-precision.
static const float PI2f = PI2;
/// \brief PI * 2 in double-precision.
static const double PI2d = 6.283185307179586476;
/// \brief PI / 2 in single-precision.
static const float PIH = 1.570796326794896619f;
/// \brief PI / 2 in single-precision.
static const float PIHf = PIH;
/// \brief PI / 2 in double-precision.
static const double PIHd = 1.570796326794896619;
/// \}
}
}
#endif

View File

@ -0,0 +1,270 @@
#ifndef _VN_MATH_CONVERSIONS_H_
#define _VN_MATH_CONVERSIONS_H_
#include "export.h"
#include "vector.h"
#include "matrix.h"
namespace vn {
namespace math {
/// \defgroup angle_convertors Angle Convertors
/// \brief Methods useful for angle conversions.
/// \{
/// \brief Converts an angle in radians to degrees.
///
/// \param[in] angleInRads The angle in radians.
/// \return The converted angle.
float rad2deg(float angleInRads);
/// \brief Converts an angle in radians to degrees.
///
/// \param[in] angleInRads The angle in radians.
/// \return The converted angle.
double rad2deg(double angleInRads);
/// \brief Convers a vector of angles in radians to degrees.
///
/// \param[in] anglesInRads The vector of angles in radians.
/// \return The converted angles.
template<size_t dim>
vec<dim, float> rad2deg(vec<dim> anglesInRads)
{
for (size_t i = 0; i < dim; i++)
anglesInRads[i] = rad2deg(anglesInRads[i]);
return anglesInRads;
}
/// \brief Converts an angle in degrees to radians.
///
/// \param[in] angleInDegs The angle in degrees.
/// \return The angle converted to radians.
float vn_proglib_DLLEXPORT deg2rad(float angleInDegs);
/// \brief Converts an angle in degrees to radians.
///
/// \param[in] angleInDegs The angle in degrees.
/// \return The angle converted to radians.
double vn_proglib_DLLEXPORT deg2rad(double angleInDegs);
/// \brief Convers a vector of angles in degrees to radians.
///
/// \param[in] anglesInDegs The vector of angles in degrees.
/// \return The converted angles.
//template<size_t dim>
//vec<dim, float> vn_proglib_DLLEXPORT deg2rad(vec<dim> anglesInDegs)
template<size_t dim>
vec<dim, float> deg2rad(vec<dim> anglesInDegs)
{
for (size_t i = 0; i < dim; i++)
anglesInDegs[i] = deg2rad(anglesInDegs[i]);
return anglesInDegs;
}
/// \}
/// \defgroup temperature_convertors Temperature Convertors
/// \brief Methods useful for temperature conversions.
/// \{
/// \brief Converts a temperature in Celsius to Fahrenheit.
///
/// \param[in] tempInCelsius The temperature in Celsius.
/// \return The converted temperature in Fahrenheit.
float celsius2fahren(float tempInCelsius);
/// \brief Converts a temperature in Celsius to Fahrenheit.
///
/// \param[in] tempInCelsius The temperature in Celsius.
/// \return The converted temperature in Fahrenheit.
double celsius2fahren(double tempInCelsius);
/// \brief Converts a temperature in Fahrenheit to Celsius.
///
/// \param[in] tempInFahren The temperature in Fahrenheit.
/// \return The converted temperature in Celsius.
float fahren2celsius(float tempInFahren);
/// \brief Converts a temperature in Fahrenheit to Celsius.
///
/// \param[in] tempInFahren The temperature in Fahrenheit.
/// \return The converted temperature in Celsius.
double fahren2celsius(double tempInFahren);
/// \brief Converts a temperature in Celsius to Kelvin.
///
/// \param[in] tempInCelsius The temperature in Celsius.
/// \return The converted temperature in Kelvin.
float celsius2kelvin(float tempInCelsius);
/// \brief Converts a temperature in Celsius to Kelvin.
///
/// \param[in] tempInCelsius The temperature in Celsius.
/// \return The converted temperature in Kelvin.
double celsius2kelvin(double tempInCelsius);
/// \brief Converts a temperature in Kelvin to Celsius.
///
/// \param[in] tempInKelvin The temperature in Kelvin.
/// \return The converted temperature in Celsius.
float kelvin2celsius(float tempInKelvin);
/// \brief Converts a temperature in Kelvin to Celsius.
///
/// \param[in] tempInKelvin The temperature in Kelvin.
/// \return The converted temperature in Celsius.
double kelvin2celsius(double tempInKelvin);
/// \brief Converts a temperature in Fahrenheit to Kelvin.
///
/// \param[in] tempInFahren The temperature in Fahrenheit.
/// \return The converted temperature in Kelvin.
float fahren2kelvin(float tempInFahren);
/// \brief Converts a temperature in Fahrenheit to Kelvin.
///
/// \param[in] tempInFahren The temperature in Fahrenheit.
/// \return The converted temperature in Kelvin.
double fahren2kelvin(double tempInFahren);
/// \brief Converts a temperature in Kelvin to Fahrenheit.
///
/// \param[in] tempInKelvin The temperature in Kelvin.
/// \return The converted temperature in Fahrenheit.
float kelvin2fahren(float tempInKelvin);
/// \brief Converts a temperature in Kelvin to Fahrenheit.
///
/// \param[in] tempInKelvin The temperature in Kelvin.
/// \return The converted temperature in Fahrenheit.
double kelvin2fahren(double tempInKelvin);
/// \}
/// \brief Converts an orientation represented as a yaw, pitch, roll in degrees to
/// a quaternion.
///
/// \param[in] yprInDegs The yaw, pitch, roll values in degrees to convert.
/// \return The corresponding quaternion.
vec4f yprInDegs2Quat(vec3f yprInDegs);
/// \brief Converts an orientation represented as a yaw, pitch, roll in radians to
/// a quaternion.
///
/// \param[in] yprInRads The yaw, pitch, roll values in radians to convert.
/// \return The corresponding quaternion.
vec4f yprInRads2Quat(vec3f yprInRads);
/// \brief Converts a yaw, pitch, roll in degrees to a direction cosine matrix.
///
/// \param[in] yprInDegs The yaw, pitch, roll values in degrees to convert.
/// \return The corresponding direction cosine matrix.
mat3f yprInDegs2Dcm(vec3f yprInDegs);
/// \brief Converts a yaw, pitch, roll in radians to a direction cosine matrix.
///
/// \param[in] yprInRads The yaw, pitch, roll values in radians to convert.
/// \return The corresponding direction cosine matrix.
mat3f yprInRads2Dcm(vec3f yprInRads);
/// \brief Converts an orientation represented as a quaternion to yaw, pitch,
/// roll values in degrees.
///
/// \param[in] The quaternion value to convert.
/// \return The corresponding yaw, pitch, roll values in degrees.
vec3f quat2YprInDegs(vec4f quat);
/// \brief Converts an orientation represented as a quaternion to yaw, pitch,
/// roll values in radians.
///
/// \param[in] The quaternion value to convert.
/// \return The corresponding yaw, pitch, roll values in radians.
vec3f quat2YprInRads(vec4f quat);
/// \brief Converts an orientation represented as a quaternion to a
/// direction cosine matrix.
///
/// \param[in] quat The quaternion to convert.
/// \return The corresponding direction cosine matrix.
mat3f quat2dcm(vec4f quat);
/// \brief Converts a direction cosine matrix to yaw, pitch, roll in degrees.
///
/// \param[in] dcm The direction cosine matrix.
/// \return The corresponding yaw, pitch, roll in degrees.
vec3f dcm2YprInDegs(mat3f dcm);
/// \brief Converts a direction cosine matrix to yaw, pitch, roll in radians.
///
/// \param[in] dcm The direction cosine matrix.
/// \return The corresponding yaw, pitch, roll in radians.
vec3f dcm2YprInRads(mat3f dcm);
/// \brief Converts an orientation represented as a direction cosine matrix to a
/// quaternion.
///
/// \param[in] dcm The direction cosine matrix to convert.
/// \return The corresponding quaternion.
vec4f dcm2quat(mat3f dcm);
/// \brief Computes the course over ground (COG) from x,y velocity components
/// in the NED frame.
///
/// \param[in] velNedX x-component of the NED velocity in m/s.
/// \param[in] velNedY y-component of the NED velocity in m/s.
/// \return The computed course over ground (COG) in radians.
float vn_proglib_DLLEXPORT course_over_ground(float velNedX, float velNedY);
/// \brief Computes the course over ground (COG) from a 3-component velocity
/// vector in NED frame.
///
/// \param[in] velNed 3-component velocity vector in NED frame in m/s.
/// \return The Computed course over ground (COG) in radians.
float vn_proglib_DLLEXPORT course_over_ground(vec3f velNed);
/// \brief Computes the speed over ground (SOG) from x,y velocity components
/// in the NED frame.
///
/// \param[in] velNedX x-component of the NED velocity in m/s.
/// \param[in] velNedY y-component of the NED velocity in m/s.
/// \return The computed speed over ground (SOG) in m/s.
float vn_proglib_DLLEXPORT speed_over_ground(float velNedX, float velNedY);
/// \brief Computes the speed over ground (SOG) from a 3-component velocity
/// vector in NED frame.
///
/// \param[in] velNed 3-component velocity vector in NED frame in m/s.
/// \return The Computed speed over ground (COG) in m/s.
float vn_proglib_DLLEXPORT speed_over_ground(vec3f velNed);
/// \brief Converts from quaternion to omega, phi, kappa representation.
///
/// \param[in] quat The quaternion.
/// \return The omega, phi, kappa representation in radians.
vec3f vn_proglib_DLLEXPORT quat2omegaPhiKappaInRads(vec4f quat);
/// \brief Converts from direction cosine matrix to omega, phi, kappa representation.
///
/// \param[in] dcm The direction cosine matrix.
/// \return The omega, phi, kappa representation in radians.
vec3f vn_proglib_DLLEXPORT dcm2omegaPhiKappaInRads(mat3f dcm);
/// \brief Converts from yaw, pitch, roll in degrees to omega, phi, kappa representation.
///
/// \param[in] yprDegs The yaw, pitch, roll in degrees.
/// \return The omega, phi, kappa representation in radians.
vec3f vn_proglib_DLLEXPORT yprInDegs2omegaPhiKappaInRads(vec3f yprDegs);
/// \brief Converts from yaw, pitch, roll in radians to omega, phi, kappa representation.
///
/// \param[in] yprRads The yaw, pitch, roll in radians.
/// \return The omega, phi, kappa representation radians.
vec3f vn_proglib_DLLEXPORT yprInRads2omegaPhiKappaInRads(vec3f yprRads);
}
}
#endif

View File

@ -0,0 +1,52 @@
/// \file
/// {COMMON_HEADER}
///
/// \section DESCRIPTION
/// This header file provides the class CriticalSection.
#ifndef _VNXPLAT_CRITICALSECTION_H_
#define _VNXPLAT_CRITICALSECTION_H_
#include "nocopy.h"
#include "export.h"
namespace vn {
namespace xplat {
/// \brief Represents a cross-platform critical section.
class vn_proglib_DLLEXPORT CriticalSection : private util::NoCopy
{
// Constructors ///////////////////////////////////////////////////////////
public:
/// \brief Creates a new critical section.
CriticalSection();
~CriticalSection();
// Public Methods /////////////////////////////////////////////////////////
public:
/// \brief Requests and signals that a critical section is being entered.
void enter();
/// \brief Signals that a critical section is being left.
void leave();
// Private Members ////////////////////////////////////////////////////////
private:
// Contains internal data, mainly stuff that is required for cross-platform
// support.
struct Impl;
Impl *_pi;
};
}
}
#endif

View File

@ -0,0 +1,66 @@
#ifndef _DLLVALIDATOR_H_
#define _DLLVALIDATOR_H_
#if defined WIN32 && defined DLL_VALIDATOR
#include <string>
#include <vector>
#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)
#endif
#include "PeLib.h"
#if defined (_MSC_VER)
#pragma warning(pop)
#endif
// Class to validate the input DLL exists and that all of it's first level
// dependencies exist as well.
class DllValidator
{
public:
DllValidator(std::string dllName, std::string currentDirectory);
bool initialize();
bool hasDllNames();
void getDllNames(std::vector<std::string>& dllNamesOut);
void getMissingDllNames(std::vector<std::string>& missingDllNamesOut);
bool validate();
private:
struct DllValidatorVisitor : public PeLib::PeFileVisitor
{
//template<int bits>
//void dumpImportDirectory(PeLib::PeFile& pef, std::vector<std::string>& dllNamesOut);
virtual void callback(PeLib::PeFile32 &file);
virtual void callback(PeLib::PeFile64 &file);
std::vector<std::string> mRequiredDlls;
};
DllValidator();
bool mIsInitialized;
bool mIsValid;
DllValidatorVisitor mVisitor;
PeLib::PeFile* mPeFile;
std::string mFileName;
std::string mWorkingDirectory;
std::vector<std::string> mMissingDlls;
};
#endif
#endif

View File

@ -0,0 +1,53 @@
#ifndef _VNDATA_ERROR_DETECTION_H_
#define _VNDATA_ERROR_DETECTION_H_
#include <cstddef>
#include <string>
#include "int.h"
namespace vn {
namespace data {
namespace integrity {
/// \brief Helpful class for working with 8-bit checksums.
class Checksum8
{
// 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 Helpful class for working with 16-bit CRCs.
class Crc16
{
// 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);
};
}
}
}
#endif

View File

@ -0,0 +1,73 @@
#ifndef _VNXPLAT_EVENT_H_
#define _VNXPLAT_EVENT_H_
#include "nocopy.h"
#include "int.h"
#include "export.h"
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.
};
// Constructors ///////////////////////////////////////////////////////////
public:
/// \brief Creates a new event.
Event();
~Event();
// 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 ////////////////////////////////////////////////////////
private:
// Contains internal data, mainly stuff that is required for cross-platform
// support.
struct Impl;
Impl *_pi;
};
}
}
#endif

View File

@ -0,0 +1,99 @@
/// \file
/// {COMMON_HEADER}
///
/// \section DESCRIPTION
/// This header file for exception classes used by the VectorNav C++ Library.
#ifndef _VN_EXCEPTIONS_H_
#define _VN_EXCEPTIONS_H_
#include <stdexcept>
#include <string>
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") { }
};
/// \brief Indicates an unknown error occurred.
class unknown_error : public std::exception
{
public:
unknown_error() : exception() { }
};
/// \brief Indicates that the requested functionality is not currently
/// implemented.
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()) { }
};
/// \brief Indicates a null pointer was provided.
class null_pointer : public std::exception
{
public:
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()) { }
};
/// \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
};
/// \brief Indicates the requested feature is not supported.
class not_supported : public std::exception
{
public:
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) { }
};
/// \brief The format was invalid.
class invalid_format : public std::exception
{
public:
invalid_format() : exception() { }
};
/// \brief A timeout occurred.
class timeout : public std::exception
{
public:
timeout() : exception() { }
/// \brief Returns a description of the exception.
///
/// \return A description of the exception.
char const* what() const throw() { return "timeout"; }
};
}
#endif

View File

@ -0,0 +1,24 @@
#ifndef _VN_UTIL_EXPORT_H
#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
#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
#else
#define vn_proglib_graphics_DLLEXPORT
#endif
#endif

View File

@ -0,0 +1,76 @@
#ifndef _VNSENSORS_EZASYNCDATA_H_
#define _VNSENSORS_EZASYNCDATA_H_
#include <string>
#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"
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);
public:
/// \brief DTOR
~EzAsyncData();
/// \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 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 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);
private:
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;
};
}
}
#endif

View File

@ -0,0 +1,28 @@
/// \file
/// {COMMON_HEADER}
///
/// \section Description
/// Provides definitions for standard integer types, typically simply including
/// the standard stdint.h file but self-defining these types if the current
/// system doesn't have this file.
#ifndef _VN_INT_H_
#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;
#else
#include <stdint.h>
#endif
#endif

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,104 @@
/// \file
/// {COMMON_HEADER}
///
/// \section DESCRIPTION
/// This header file provides the class MemoryPort.
#ifndef _MEMORYPORT_H_
#define _MEMORYPORT_H_
#if defined(_MSC_VER)
#pragma warning(push)
#pragma warning(disable:4996)
#endif
#include <string>
#if defined(_MSC_VER)
#pragma warning(pop)
#endif
#include "int.h"
#include "port.h"
#include "nocopy.h"
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);
// Constructors ///////////////////////////////////////////////////////////
public:
/// \brief Creates a new \ref MemoryPort.
MemoryPort();
~MemoryPort();
// Public Methods /////////////////////////////////////////////////////////
public:
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 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 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 The data to send.
void SendDataBackDoor(const std::string data);
// Private Members ////////////////////////////////////////////////////////
private:
// Contains internal data, mainly stuff that is required for support.
struct Impl;
Impl *_pi;
};
}
}
#endif

View File

@ -0,0 +1,10 @@
#ifndef _VNSENSORS_MOCK_H_
#define _VNSENSORS_MOCK_H_
namespace xplat {
namespace sensors {
}
}
#endif

View File

@ -0,0 +1,45 @@
/// \file
/// {COMMON_HEADER}
#ifndef _VNUTIL_NOCOPY_H_
#define _VNUTIL_NOCOPY_H_
#include "export.h"
namespace vn {
namespace util {
/// \brief Identifies a derived class as being unable to be copied and prevents
/// copy attempts.
///
/// Example for making derived objects uncopyable.
/// \code
/// class MyClass : private NoCopy
/// {
/// // Class implementation.
/// }
/// \endcode
class vn_proglib_DLLEXPORT NoCopy
{
protected:
/// \brief Allows construction of derived objects.
NoCopy() { }
/// \brief Allows destruction of derived objects.
~NoCopy() { }
private:
/// \brief Prevent copying of derived objects.
NoCopy(const NoCopy&);
/// \brief Prevent assignment copying of derived objects.
NoCopy& operator=(const NoCopy&);
};
}
}
#endif

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,103 @@
#ifndef _VNPROTOCOL_UART_PACKETFINDER_H_
#define _VNPROTOCOL_UART_PACKETFINDER_H_
#include "nocopy.h"
#include "vntime.h"
#include "packet.h"
#if PYTHON
#include "vn/util/boostpython.h"
#endif
namespace vn {
namespace protocol {
namespace uart {
/// \brief Helps with management of communication with a sensor using the UART
/// protocol.
///
/// Internally, the PacketFinder keeps track of a running data index which
/// keeps a running count of the bytes that are processed by the class. This is
/// useful for users who wish to keep track of where packets where found in the
/// incoming raw data stream. When the PacketFinder receives its first byte
/// from the user, this is given the index of 0 for the running index and
/// 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);
#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;
};
}
}
}
#endif

View File

@ -0,0 +1,82 @@
#ifndef _VN_XPLAT_PORT_H_
#define _VN_XPLAT_PORT_H_
#include <cstddef>
#include "export.h"
namespace vn {
namespace xplat {
/// \brief Interface for a simple port.
class vn_proglib_DLLEXPORT IPort
{
// 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);
// 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 <c>true</c> if the serial port is open; otherwise <c>false</c>.
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

View File

@ -0,0 +1,50 @@
#ifndef _VNMATH_POSITION_H_
#define _VNMATH_POSITION_H_
#include "vector.h"
namespace vn {
namespace math {
/// \brief Representation of a position/location.
class PositionD
{
private:
enum PositionType
{
POS_LLA,
POS_ECEF
};
public:
// TEMP
PositionD() { }
private:
PositionD(PositionType type, vec3d pos);
public:
/// \brief Creates a new <c>PositionD</c> from a latitude, longitude, altitude.
///
/// \param[in] lla The position expressed as a latitude, longitude, altitude.
/// \return The new <c>PositionD</c>.
static PositionD fromLla(vec3d lla);
/// \brief Creates a new <c>PositionD</c> from an earth-centered, earth-fixed.
///
/// \param[in] ecef The position expressed as an earth-centered, earth-fixed.
/// \return The new <c>PositionD</c>.
static PositionD fromEcef(vec3d ecef);
private:
PositionType _underlyingType;
vec3d _data;
};
}
}
#endif

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,101 @@
#ifndef _VN_XPLAT_RTCMLISTENER_H_
#define _VN_XPLAT_RTCMLISTENER_H_
#include "rtcmmessage.h"
#include "export.h"
#include <functional> // Used for the call back function
#include <string.h>
#ifdef _WIN32
#ifndef _WIN32_WINNT
#define _WIN32_WINNT 0x0501
#endif
#include <winsock2.h>
#include <ws2tcpip.h>
#else
#include <sys/socket.h> // Use POSIX-style sockets
#include <arpa/inet.h>
#include <netdb.h> // Needed for getaddrinfo() and freeaddrinfo()
#include <unistd.h> // Needed for close()
//#include <sys/types.h>
#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<void(rtcmmessage)> 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())
#else
#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";
// 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<std::thread> serverThread = nullptr;
Thread* serverThread = nullptr;
std::function<void(rtcmmessage)> messageNotification;
bool messageNotificationActive = false;
};
}
}
#endif

View File

@ -0,0 +1,82 @@
#ifndef _VN_XPLAT_RTCMMESSAGE_H_
#define _VN_XPLAT_RTCMMESSAGE_H_
#include <string>
#include <string.h>
#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;
/// \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);
// 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
};
};
}
}
#endif

View File

@ -0,0 +1,62 @@
#ifndef _VNSENSORS_SEARCHER_H_
#define _VNSENSORS_SEARCHER_H_
#include <string>
#include <vector>
#include "int.h"
#include "export.h"
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 <c>true</c> if a sensor if found; otherwise <c>false</c>.
static void findPorts(std::vector<std::string>& 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 <c>true</c> if a sensor if found; otherwise <c>false</c>.
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<std::pair<std::string, uint32_t> > 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<std::pair<std::string, uint32_t> > search(std::vector<std::string>& 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 <c>true</c> if a sensor if found; otherwise <c>false</c>.
static bool test(std::string portName, uint32_t baudrate);
};
}
}
#endif

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,154 @@
#ifndef _VN_XPLAT_SERIALPORT_H_
#define _VN_XPLAT_SERIALPORT_H_
#if _MSC_VER && _WIN32
#pragma comment(lib, "setupapi.lib")
#endif
#include <string>
#include <vector>
#include <list>
#if PYTHON
#include "boostpython.h"
#endif
#include "int.h"
#include "port.h"
#include "nocopy.h"
#include "export.h"
namespace vn {
namespace xplat {
/// \brief Represents a cross-platform serial port.
///
/// When the SerialPort if first created and the connection opened, the user
/// will normally have to poll the method \ref read to see if any new data is
/// available on the serial port. However, if the user code registers a
/// handler with the method \ref registerDataReceivedHandler, the SerialPort
/// object will start an internal thread that monitors the serial port for new
/// data, and when new data is available, it will alert the user code through
/// the callback handler. Then the user can call \ref read to retrieve the
/// data.
class vn_proglib_DLLEXPORT SerialPort : public IPort, util::NoCopy
{
// Types //////////////////////////////////////////////////////////////////
public:
enum StopBits
{
ONE_STOP_BIT,
TWO_STOP_BITS
};
// 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);
~SerialPort();
// 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<std::string> 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 <c>true</c> if the COM port is optimized; otherwise <c>false</c>.
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;
};
}
}
#endif

View File

@ -0,0 +1,91 @@
#ifndef _VNXPLAT_SIGNAL_H_
#define _VNXPLAT_SIGNAL_H_
namespace vn {
namespace xplat {
/// \brief Provides access to system signals.
class Signal
{
// Types //////////////////////////////////////////////////////////////////
public:
/// \brief The available signals.
enum SignalType
{
/// Unknown type of signal.
UNKNOWN,
/// User pressed <em>Ctrl-C</em>.
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 ////////////////////////////////////////////////////////
private:
// Contains internal data, mainly stuff that is required for cross-platform
// support.
struct Impl;
};
}
}
#endif

View File

@ -0,0 +1,108 @@
/// \file
/// {COMMON_HEADER}
///
/// \section DESCRIPTION
/// This header file provides the class Thread.
#ifndef _VNXPLAT_THREAD_H_
#define _VNXPLAT_THREAD_H_
#include "int.h"
#include "export.h"
#include "nocopy.h"
#if _WIN32
#include <Windows.h>
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__
#include <pthread.h>
#include <unistd.h>
#else
#error "Unknown System"
#endif
namespace vn {
namespace xplat {
/// \brief Represents a cross-platform thread.
class vn_proglib_DLLEXPORT Thread : private util::NoCopy
{
// Types //////////////////////////////////////////////////////////////////
public:
/// \brief Represents a start routine for a thread which will have data
/// passed to it.
typedef void (*ThreadStartRoutine)(void*);
// Constructors ///////////////////////////////////////////////////////////
public:
~Thread();
private:
/// \brief Creates a new <c>Thread</c> object.
///
/// \param[in] startRoutine The routine to call when the thread is started.
explicit Thread(ThreadStartRoutine startRoutine);
// 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 <c>Thread</c> 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;
};
}
}
#endif

View File

@ -0,0 +1,705 @@
#ifndef _VNPROTOCOL_UART_TYPES_H_
#define _VNPROTOCOL_UART_TYPES_H_
#include "int.h"
namespace vn {
namespace protocol {
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
};
/// \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.
};
/// \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.
};
/// \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.
};
/// \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.
};
/// \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.
};
/// \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.
};
/// \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.
};
/// \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.
};
/// \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
};
/// \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
};
/// \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
};
/// \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
};
/// \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
};
/// \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
};
/// \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
};
/// \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
};
/// \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
};
/// \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
};
/// \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
};
/// \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
};
/// \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
};
/// \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
};
/// \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
};
/// \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
};
/// \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
};
/// \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
};
/// \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
};
/// \brief VPE Enable mode for the VPE Basic Control register.
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
};
/// \brief VPE modes for the VPE Basic Control register.
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
};
/// \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
};
/// \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
};
/// \brief Type of velocity compensation performed by the VPE.
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
};
/// \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
};
/// \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
};
/// \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.
};
/// \brief Status indicators for VPE.
struct VpeStatus
{
/// \brief AttitudeQuality field.
uint8_t attitudeQuality;
/// \brief GyroSaturation field.
bool gyroSaturation;
/// \brief GyroSaturationRecovery field.
bool gyroSaturationRecovery;
/// \brief MagDistrubance field.
uint8_t magDisturbance;
/// \brief MagSaturation field.
bool magSaturation;
/// \brief AccDisturbance field.
uint8_t accDisturbance;
/// \brief AccSaturation field.
bool accSaturation;
/// \brief KnownMagDisturbance field.
bool knownMagDisturbance;
/// \brief KnownAccelDisturbance field.
bool knownAccelDisturbance;
/// \brief Default constructor.
VpeStatus();
/// \brief Constructs a <c>VpeStatus</c> 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.
};
/// \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.
/// \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.
};
/// \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.
};
/// \brief Allows combining flags of the CommonGroup enum.
///
/// \param[in] lhs Left-hand side enum value.
/// \param[in] rhs Right-hand side enum value.
/// \return The binary ORed value.
CommonGroup operator|(CommonGroup lhs, CommonGroup rhs);
/// \brief Allows combining flags of the TimeGroup enum.
///
/// \param[in] lhs Left-hand side enum value.
/// \param[in] rhs Right-hand side enum value.
/// \return The binary ORed value.
TimeGroup operator|(TimeGroup lhs, TimeGroup rhs);
/// \brief Allows combining flags of the ImuGroup enum.
///
/// \param[in] lhs Left-hand side enum value.
/// \param[in] rhs Right-hand side enum value.
/// \return The binary ORed value.
ImuGroup operator|(ImuGroup lhs, ImuGroup rhs);
/// \brief Allows combining flags of the GpsGroup enum.
///
/// \param[in] lhs Left-hand side enum value.
/// \param[in] rhs Right-hand side enum value.
/// \return The binary ORed value.
GpsGroup operator|(GpsGroup lhs, GpsGroup rhs);
/// \brief Allows combining flags of the AttitudeGroup enum.
///
/// \param[in] lhs Left-hand side enum value.
/// \param[in] rhs Right-hand side enum value.
/// \return The binary ORed value.
AttitudeGroup operator|(AttitudeGroup lhs, AttitudeGroup rhs);
/// \brief Allows combining flags of the InsGroup enum.
///
/// \param[in] lhs Left-hand side enum value.
/// \param[in] rhs Right-hand side enum value.
/// \return The binary ORed value.
InsGroup operator|(InsGroup lhs, InsGroup rhs);
}
}
}
#endif

View File

@ -0,0 +1,445 @@
#ifndef _VNPROTOCOL_UART_UTIL_H_
#define _VNPROTOCOL_UART_UTIL_H_
#include <string>
#include <sstream>
#include "types.h"
namespace vn {
namespace protocol {
namespace uart {
// Utility functions.
/// \brief Converts an AsciiAsync enum into a string.
///
/// \param[in] val The AsciiAsync enum value to convert to string.
/// \return The converted value.
std::string str(AsciiAsync val);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// AsciiAsync enums.
///
/// \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);
/// \brief Converts a SensorError enum into a string.
///
/// \param[in] val The SensorError enum value to convert to string.
/// \return The converted value.
std::string str(SensorError val);
/// \brief Converts a BootloaderError enum into a string.
///
/// \param[in] val The BootloaderError enum value to convert to string.
/// \return The converted value.
std::string str(BootloaderError val);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// SensorError enums.
///
/// \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);
/// \brief Converts a SyncInMode enum into a string.
///
/// \param[in] val The SyncInMode enum value to convert to string.
/// \return The converted value.
std::string str(SyncInMode val);
/// \brief Converts a SyncInEdge enum into a string.
///
/// \param[in] val The SyncInEdge enum value to convert to string.
/// \return The converted value.
std::string str(SyncInEdge val);
/// \brief Converts a SyncOutMode enum into a string.
///
/// \param[in] val The SyncOutMode enum value to convert to string.
/// \return The converted value.
std::string str(SyncOutMode val);
/// \brief Converts a SyncOutPolarity enum into a string.
///
/// \param[in] val The SyncOutPolarity enum value to convert to string.
/// \return The converted value.
std::string str(SyncOutPolarity val);
/// \brief Converts a CountMode enum into a string.
///
/// \param[in] val The CountMode enum value to convert to string.
/// \return The converted value.
std::string str(CountMode val);
/// \brief Converts a StatusMode enum into a string.
///
/// \param[in] val The StatusMode enum value to convert to string.
/// \return The converted value.
std::string str(StatusMode val);
/// \brief Converts a ChecksumMode enum into a string.
///
/// \param[in] val The ChecksumMode enum value to convert to string.
/// \return The converted value.
std::string str(ChecksumMode val);
/// \brief Converts a ErrorMode enum into a string.
///
/// \param[in] val The ErrorMode enum value to convert to string.
/// \return The converted value.
std::string str(ErrorMode val);
/// \brief Converts a FilterMode enum into a string.
///
/// \param[in] val The FilterMode enum value to convert to string.
/// \return The converted value.
std::string str(FilterMode val);
/// \brief Converts a IntegrationFrame enum into a string.
///
/// \param[in] val The IntegrationFrame enum value to convert to string.
/// \return The converted value.
std::string str(IntegrationFrame val);
/// \brief Converts a CompensationMode enum into a string.
///
/// \param[in] val The CompensationMode enum value to convert to string.
/// \return The converted value.
std::string str(CompensationMode val);
/// \brief Converts a AccCompensationMode enum into a string.
///
/// \param[in] val The AccCompensationMode enum value to convert to string.
/// \return The converted value.
std::string str(AccCompensationMode val);
/// \brief Converts a EarthRateCorrection enum into a string.
///
/// \param[in] val The EarthRateCorrection enum value to convert to string.
/// \return The converted value.
std::string str(EarthRateCorrection val);
/// \brief Converts a GpsFix enum into a string.
///
/// \param[in] val The GpsFix enum value to convert to string.
/// \return The converted value.
std::string str(GpsFix val);
/// \brief Converts a GpsMode enum into a string.
///
/// \param[in] val The GpsMode enum value to convert to string.
/// \return The converted value.
std::string str(GpsMode val);
/// \brief Converts a PpsSource enum into a string.
///
/// \param[in] val The PpsSource enum value to convert to string.
/// \return The converted value.
std::string str(PpsSource val);
/// \brief Converts a GpsRate enum into a string.
///
/// \param[in] val The GpsRate enum value to convert to string.
/// \return The converted value.
std::string str(GpsRate val);
/// \brief Converts a AntPower enum into a string.
///
/// \param[in] val The AntPower enum value to convert to string.
/// \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.
/// \return The converted value.
std::string str(VpeEnable val);
/// \brief Converts a HeadingMode enum into a string.
///
/// \param[in] val The HeadingMode enum value to convert to string.
/// \return The converted value.
std::string str(HeadingMode val);
/// \brief Converts a VpeMode enum into a string.
///
/// \param[in] val The VpeMode enum value to convert to string.
/// \return The converted value.
std::string str(VpeMode val);
/// \brief Converts a Scenario enum into a string.
///
/// \param[in] val The Scenario enum value to convert to string.
/// \return The converted value.
std::string str(Scenario val);
/// \brief Converts a HsiMode enum into a string.
///
/// \param[in] val The HsiMode enum value to convert to string.
/// \return The converted value.
std::string str(HsiMode val);
/// \brief Converts a HsiOutput enum into a string.
///
/// \param[in] val The HsiOutput enum value to convert to string.
/// \return The converted value.
std::string str(HsiOutput val);
/// \brief Converts a VelocityCompensationMode enum into a string.
///
/// \param[in] val The VelocityCompensationMode enum value to convert to string.
/// \return The converted value.
std::string str(VelocityCompensationMode val);
/// \brief Converts a MagneticMode enum into a string.
///
/// \param[in] val The MagneticMode enum value to convert to string.
/// \return The converted value.
std::string str(MagneticMode val);
/// \brief Converts a ExternalSensorMode enum into a string.
///
/// \param[in] val The ExternalSensorMode enum value to convert to string.
/// \return The converted value.
std::string str(ExternalSensorMode val);
/// \brief Converts a FoamInit enum into a string.
///
/// \param[in] val The FoamInit enum value to convert to string.
/// \return The converted value.
std::string str(FoamInit val);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// SyncInMode enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// SyncInEdge enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// SyncOutMode enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// SyncOutPolarity enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// CountMode enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// StatusMode enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// ChecksumMode enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// ErrorMode enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// FilterMode enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// IntegrationFrame enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// CompensationMode enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// AccCompensationMode enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// EarthRateCorrection enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// GpsFix enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// GpsMode enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// PpsSource enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// GpsRate enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// AntPower enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// VpeEnable enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// HeadingMode enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// VpeMode enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// Scenario enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// HsiMode enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// HsiOutput enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// VelocityCompensationMode enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// MagneticMode enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// ExternalSensorMode enums.
///
/// \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);
/// \brief Overloads the ostream << operator for easy usage in displaying
/// FoamInit enums.
///
/// \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);
}
}
}
#endif

View File

@ -0,0 +1,128 @@
/// \file
/// {COMMON_HEADER}
#ifndef _VN_UTILITIES_H_
#define _VN_UTILITIES_H_
#include <string>
#include <vector>
#include "int.h"
#include "export.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
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 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);
/// \brief Converts a 16-bit integer in sensor order to host order.
///
/// \param[in] sensorOrdered The 16-bit integer in sensor order.
/// \return The value converted to host ordered.
uint16_t stoh(uint16_t sensorOrdered);
/// \brief Converts a 32-bit integer in sensor order to host order.
///
/// \param[in] sensorOrdered The 32-bit integer in sensor order.
/// \return The value converted to host ordered.
uint32_t stoh(uint32_t sensorOrdered);
/// \brief Converts a 64-bit integer in sensor order to host order.
///
/// \param[in] sensorOrdered The 64-bit integer in sensor order.
/// \return The value converted to host ordered.
uint64_t stoh(uint64_t sensorOrdered);
/// \brief Counts the number of bits set in the provided value.
///
/// \param[in] d The value to count the bits of.
/// \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.
/// \return The converted value.
uint8_t to_uint8_from_hexchar(char c);
/// \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 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);
#if PYTHON
/// \brief Checks a DLL's first level dependencies to see if they are all
/// present. If any of the DLLs cannot be found the software will
/// print the missing DLLs to the console
///
/// \param[in] dllName absolute or relative path to the dll in question
/// \param[in] workingDirectory current working directory. Must be the
/// same one as the program or results may be incorrect
/// \param[out] missingDlls vector of stings to hold the names of any missing
/// Dlls. This vector will be emptied if all Dlls are
/// 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<std::string>& missingDlls);
#endif
}
#endif

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,60 @@
#ifndef _VNXPLAT_TIME_H_
#define _VNXPLAT_TIME_H_
#include "int.h"
#include "export.h"
namespace vn {
namespace xplat {
struct vn_proglib_DLLEXPORT TimeStamp
{
public:
TimeStamp();
private:
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:
public:
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();
~Stopwatch();
/// \brief Resets the Stopwatch.
void reset();
/// \brief Gets the elapsed time in milliseconds.
///
/// \return The elapsed time in milliseconds.
float elapsedMs();
private:
struct Impl;
Impl *_pi;
};
}
}
#endif