From 3349c89930cdb021f4394efa99bf3250f7daa047 Mon Sep 17 00:00:00 2001 From: Andre Nguyen Date: Thu, 25 Feb 2016 13:10:46 -0500 Subject: [PATCH 01/10] add beta library --- CMakeLists.txt | 11 +- ext/vnproglib-1.1.0.115/CMakeLists.txt | 287 ++ ext/vnproglib-1.1.0.115/download.md | 1 + .../include/vn/data/error_detection.h | 55 + ext/vnproglib-1.1.0.115/include/vn/event.h | 290 ++ .../include/vn/exceptions.h | 104 + ext/vnproglib-1.1.0.115/include/vn/int.h | 30 + .../include/vn/math/Coordinate2.h | 49 + .../include/vn/math/attitude.h | 85 + .../include/vn/math/consts.h | 50 + .../include/vn/math/conversions.h | 248 ++ .../include/vn/math/kinematics.h | 262 ++ .../include/vn/math/math.h | 16 + .../include/vn/math/matrix.h | 1679 +++++++ .../include/vn/math/position.h | 48 + .../include/vn/math/vector.h | 1377 ++++++ ext/vnproglib-1.1.0.115/include/vn/math/ypr.h | 89 + .../include/vn/protocol/uart/packet.h | 2108 +++++++++ .../include/vn/protocol/uart/packetfinder.h | 104 + .../include/vn/protocol/uart/types.h | 612 +++ .../include/vn/protocol/uart/util.h | 383 ++ .../include/vn/python/util.h | 83 + .../include/vn/sensors/compositedata.h | 669 +++ .../include/vn/sensors/ezasyncdata.h | 75 + .../include/vn/sensors/mock.h | 12 + .../include/vn/sensors/registers.h | 1462 ++++++ .../include/vn/sensors/searcher.h | 55 + .../include/vn/sensors/sensors.h | 1436 ++++++ .../include/vn/util/boostpython.h | 25 + .../include/vn/util/compiler.h | 81 + .../include/vn/util/dllvalidator.h | 68 + .../include/vn/util/export.h | 26 + .../include/vn/util/memoryport.h | 106 + .../include/vn/util/nocopy.h | 47 + .../include/vn/utilities.h | 130 + .../include/vn/xplat/criticalsection.h | 54 + .../include/vn/xplat/event.h | 75 + .../include/vn/xplat/port.h | 84 + .../include/vn/xplat/serialport.h | 216 + .../include/vn/xplat/signal.h | 93 + .../include/vn/xplat/thread.h | 101 + .../include/vn/xplat/time.h | 61 + ext/vnproglib-1.1.0.115/src/vn/benchmark.cpp | 28 + .../src/vn/data/error_detection.cpp | 47 + ext/vnproglib-1.1.0.115/src/vn/event.cpp | 77 + ext/vnproglib-1.1.0.115/src/vn/main.cpp | 22 + .../src/vn/math/attitude.cpp | 122 + .../src/vn/math/conversions.cpp | 295 ++ .../src/vn/math/position.cpp | 26 + .../src/vn/math/python.cpp | 1604 +++++++ .../src/vn/protocol/uart/packet.cpp | 3966 +++++++++++++++++ .../src/vn/protocol/uart/packetfinder.cpp | 564 +++ .../src/vn/protocol/uart/python.cpp | 1673 +++++++ .../src/vn/protocol/uart/types.cpp | 59 + .../src/vn/protocol/uart/util.cpp | 661 +++ ext/vnproglib-1.1.0.115/src/vn/python.cpp | 240 + .../src/vn/sensors/compositedata.cpp | 2161 +++++++++ .../src/vn/sensors/ezasyncdata.cpp | 104 + .../src/vn/sensors/python.cpp | 986 ++++ .../src/vn/sensors/searcher.cpp | 202 + .../src/vn/sensors/sensors.cpp | 3392 ++++++++++++++ .../src/vn/util/dllvalidator.cpp | 125 + .../src/vn/util/memoryport.cpp | 190 + ext/vnproglib-1.1.0.115/src/vn/utilities.cpp | 205 + .../src/vn/xplat/criticalsection.cpp | 83 + .../src/vn/xplat/event.cpp | 272 ++ ext/vnproglib-1.1.0.115/src/vn/xplat/port.cpp | 11 + .../src/vn/xplat/python.cpp | 103 + .../src/vn/xplat/serialport.cpp | 1405 ++++++ .../src/vn/xplat/thread.cpp | 184 + ext/vnproglib-1.1.0.115/src/vn/xplat/time.cpp | 183 + src/imu_vn_100.cpp | 2 +- 72 files changed, 31835 insertions(+), 4 deletions(-) create mode 100755 ext/vnproglib-1.1.0.115/CMakeLists.txt create mode 100644 ext/vnproglib-1.1.0.115/download.md create mode 100755 ext/vnproglib-1.1.0.115/include/vn/data/error_detection.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/event.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/exceptions.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/int.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/math/Coordinate2.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/math/attitude.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/math/consts.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/math/conversions.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/math/kinematics.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/math/math.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/math/matrix.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/math/position.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/math/vector.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/math/ypr.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/protocol/uart/packet.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/protocol/uart/packetfinder.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/protocol/uart/types.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/protocol/uart/util.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/python/util.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/sensors/compositedata.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/sensors/ezasyncdata.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/sensors/mock.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/sensors/registers.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/sensors/searcher.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/sensors/sensors.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/util/boostpython.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/util/compiler.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/util/dllvalidator.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/util/export.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/util/memoryport.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/util/nocopy.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/utilities.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/xplat/criticalsection.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/xplat/event.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/xplat/port.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/xplat/serialport.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/xplat/signal.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/xplat/thread.h create mode 100755 ext/vnproglib-1.1.0.115/include/vn/xplat/time.h create mode 100755 ext/vnproglib-1.1.0.115/src/vn/benchmark.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/data/error_detection.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/event.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/main.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/math/attitude.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/math/conversions.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/math/position.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/math/python.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/protocol/uart/packet.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/protocol/uart/packetfinder.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/protocol/uart/python.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/protocol/uart/types.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/protocol/uart/util.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/python.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/sensors/compositedata.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/sensors/ezasyncdata.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/sensors/python.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/sensors/searcher.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/sensors/sensors.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/util/dllvalidator.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/util/memoryport.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/utilities.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/xplat/criticalsection.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/xplat/event.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/xplat/port.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/xplat/python.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/xplat/serialport.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/xplat/thread.cpp create mode 100755 ext/vnproglib-1.1.0.115/src/vn/xplat/time.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 30ddc2f..12e77d7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(imu_vn_100) +add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/ext/vnproglib-1.1.0.115) add_definitions("-std=c++11") find_package(catkin REQUIRED COMPONENTS @@ -11,8 +12,8 @@ find_package(catkin REQUIRED COMPONENTS find_package(Boost REQUIRED) catkin_package( - INCLUDE_DIRS include vncpplib/include - LIBRARIES imu_vn_100 + INCLUDE_DIRS include vncpplib/include ext/vnproglib-1.1.0.115/include + LIBRARIES imu_vn_100 vnproglib CATKIN_DEPENDS diagnostic_updater roscpp sensor_msgs DEPENDS boost ) @@ -20,19 +21,21 @@ catkin_package( include_directories( include vncpplib/include + ext/vnproglib-1.1.0.115/include ${catkin_INCLUDE_DIRS} ) - add_library(imu_vn_100 vncpplib/src/arch/linux/vncp_services.c vncpplib/src/vndevice.c vncpplib/src/vn100.c src/imu_vn_100.cpp ) + target_link_libraries(imu_vn_100 ${catkin_LIBRARIES} ) + add_dependencies(imu_vn_100 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} @@ -40,8 +43,10 @@ add_dependencies(imu_vn_100 add_executable(imu_vn_100_cont_node src/imu_vn_100_cont.cpp) + target_link_libraries(imu_vn_100_cont_node ${catkin_LIBRARIES} + vnproglib imu_vn_100 ) add_dependencies(imu_vn_100_cont_node diff --git a/ext/vnproglib-1.1.0.115/CMakeLists.txt b/ext/vnproglib-1.1.0.115/CMakeLists.txt new file mode 100755 index 0000000..1666e36 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/CMakeLists.txt @@ -0,0 +1,287 @@ +cmake_minimum_required(VERSION 2.8.4) + +set(CMAKE_SUPPRESS_REGENERATION TRUE) +add_definitions(-DPL150) +#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +#set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) +#set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) +#set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin5) +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}) + +option(BUILD_TESTS "Build tests." OFF) +option(BUILD_BENCHMARKS "Build benchmarks." OFF) +option(PYTHON "Build for Python library." OFF) +option(BUILD_GRAPHICS "Build in the graphics library." OFF) +#option(INTERNAL "Build internal portions of the library." OFF) + +set_property(GLOBAL PROPERTY USE_FOLDERS ON) + +set(top_sources + include/vn/exceptions.h + include/vn/int.h + include/vn/utilities.h + include/vn/event.h + src/vn/utilities.cpp + src/vn/event.cpp) +source_group("" FILES ${top_sources}) + +set(math_sources + include/vn/math/consts.h + include/vn/math/math.h + include/vn/math/conversions.h + include/vn/math/vector.h + include/vn/math/matrix.h + include/vn/math/kinematics.h + include/vn/math/attitude.h + include/vn/math/position.h + src/vn/math/conversions.cpp + src/vn/math/attitude.cpp + src/vn/math/position.cpp) +source_group("math" FILES ${math_sources}) + +set(util_sources + include/vn/util/export.h + include/vn/util/boostpython.h + include/vn/util/compiler.h + include/vn/util/memoryport.h + include/vn/util/nocopy.h + src/vn/util/memoryport.cpp) +#if (PYTHON) +# set(util_sources +# ${util_sources} +# include/vn/util/dllvalidator.h +# src/vn/util/dllvalidator.cpp) +#endif() +source_group("util" FILES ${util_sources}) + +set(data_sources + include/vn/data/error_detection.h + src/vn/data/error_detection.cpp) +source_group("data" FILES ${data_sources}) + +set(protocol_uart_sources + include/vn/protocol/uart/types.h + include/vn/protocol/uart/util.h + include/vn/protocol/uart/packet.h + include/vn/protocol/uart/packetfinder.h + src/vn/protocol/uart/types.cpp + src/vn/protocol/uart/util.cpp + src/vn/protocol/uart/packet.cpp + src/vn/protocol/uart/packetfinder.cpp) +source_group("protocol\\uart" FILES ${protocol_uart_sources}) + +set(sensors_sources + include/vn/sensors/registers.h + include/vn/sensors/mock.h + include/vn/sensors/sensors.h + include/vn/sensors/searcher.h + include/vn/sensors/compositedata.h + include/vn/sensors/ezasyncdata.h + src/vn/sensors/sensors.cpp + src/vn/sensors/searcher.cpp + src/vn/sensors/compositedata.cpp + src/vn/sensors/ezasyncdata.cpp) +source_group("sensors" FILES ${sensors_sources}) + +set(xplat_sources + include/vn/xplat/criticalsection.h + include/vn/xplat/event.h + include/vn/xplat/serialport.h + include/vn/xplat/signal.h + include/vn/xplat/thread.h + include/vn/xplat/time.h + include/vn/xplat/port.h + src/vn/xplat/criticalsection.cpp + src/vn/xplat/event.cpp + src/vn/xplat/serialport.cpp + src/vn/xplat/thread.cpp + src/vn/xplat/time.cpp + src/vn/xplat/port.cpp) +source_group("xplat" FILES ${xplat_sources}) + +set(graphics_sources + include/vn/graphics/color.h + include/vn/graphics/sphere.h + include/vn/graphics/geometry.h + include/vn/graphics/multicolor.h + include/vn/graphics/lines.h + include/vn/graphics/math.h + include/vn/graphics/frame.h + src/vn/graphics/color.cpp + src/vn/graphics/sphere.cpp + src/vn/graphics/geometry.cpp + src/vn/graphics/multicolor.cpp + src/vn/graphics/lines.cpp + src/vn/graphics/math.cpp + src/vn/graphics/util.cpp + src/vn/graphics/frame.cpp) +source_group("" FILES ${graphics_sources}) + +set(graphics_math_sources + include/vn/graphics/math/vector.h + include/vn/graphics/math/matrix.h) +source_group("math" FILES ${graphics_math_sources}) + + +include_directories(include) + +if (UNIX) + include_directories( + /user/include/python3.4) +endif() + +#if (PYTHON) +# # Python needs this as a DLL to load into the Python interpretor. +# add_library(proglib-cpp SHARED +# ${top_sources} +# ${math_sources} +# ${util_sources} +# ${data_sources} +# ${protocol_uart_sources} +# ${sensors_sources} +# ${xplat_sources}) +# +# if(WIN32) +# target_compile_definitions(proglib-cpp PUBLIC -D_BUILD_DLL) +# target_compile_definitions(proglib-cpp PUBLIC -Dproglib_cpp_EXPORTS) +# +# set_target_properties( +# proglib-cpp PROPERTIES +# RUNTIME_OUTPUT_DIRECTORY "../../vnpy" +# RUNTIME_OUTPUT_DIRECTORY_RELEASE "../../vnpy" +# RUNTIME_OUTPUT_DIRECTORY_DEBUG "../../vnpy" +# LIBRARY_OUTPUT_DIRECTORY "../../vnpy") +# +# include_directories(../libs/PeLib/include) +# target_link_libraries(proglib-cpp LINK_PUBLIC PeLib) +# add_subdirectory(../libs/PeLib PeLib) +# +# set_target_properties( +# PeLib PROPERTIES +# RUNTIME_OUTPUT_DIRECTORY "../../../vnpy" +# RUNTIME_OUTPUT_DIRECTORY_RELEASE "../../../vnpy" +# RUNTIME_OUTPUT_DIRECTORY_DEBUG "../../../vnpy" +# LIBRARY_OUTPUT_DIRECTORY "../../../vnpy") +# elseif(UNIX) +# set_target_properties( +# proglib-cpp PROPERTIES +# RUNTIME_OUTPUT_DIRECTORY "../vnpy" +# RUNTIME_OUTPUT_DIRECTORY_RELEASE "../vnpy" +# RUNTIME_OUTPUT_DIRECTORY_DEBUG "../vnpy" +# LIBRARY_OUTPUT_DIRECTORY "../vnpy") +# endif() +#else() +add_library(vnproglib + ${top_sources} + ${math_sources} + ${util_sources} + ${data_sources} + ${protocol_uart_sources} + ${sensors_sources} + ${xplat_sources}) +# +# if(WIN32) +# target_link_libraries(proglib-cpp PRIVATE Setupapi) +# endif() +#endif() +# +#if (BUILD_GRAPHICS) +# +# include_directories( +# ../libs/glew/include +# ../libs/glm) +# +# +# #target_link_libraries(proglib-cpp-graphics PRIVATE GLEW_shared) +# +## set(CMAKE_PREFIX_PATH "../libs/glew") +## set(CMAKE_LIBRARY_PATH "../libs/glew/lib/Release/Win32/") +# +# #message("DIR: ${GLEW_INCLUDE_DIR}") +## find_package(GLEW REQUIRED) +# #add_library(GLEW_shared INTERFACE IMPORTED) +# #message("DIR: ${GLEW_INCLUDE_DIR}") +# +# #message("${GLEW_FOUND}") +# +## include_directories(${GLEW_INCLUDE_DIRS}) +## link_libraries(${GLEW_LIBRARIES}) +# +# +## add_subdirectory(../libs/glew GLEW_shared) +# +# +# add_library(proglib-cpp-graphics SHARED +# ${graphics_sources} +# ${graphics_math_sources}) +# target_compile_definitions(proglib-cpp-graphics PUBLIC -D_BUILD_DLL) +# target_compile_definitions(proglib-cpp-graphics PUBLIC -Dproglib_cpp_graphics_EXPORTS) +# target_link_libraries(proglib-cpp-graphics PRIVATE OpenGL32) +# target_link_libraries(proglib-cpp-graphics PRIVATE GLEW_shared) +# +#endif() +# +#if (WIN32) +# target_link_libraries(proglib-cpp PRIVATE Setupapi) + +# if (PYTHON) +# include_directories(../libs/PeLib/include) +# target_link_libraries(proglib-cpp LINK_PUBLIC PeLib) +# add_subdirectory(../libs/PeLib PeLib) +# endif() +#endif() + + +# The properties below are mainly for the Python library. +#if (WIN32) +#set_target_properties( +# proglib-cpp PROPERTIES +# RUNTIME_OUTPUT_DIRECTORY "../../../vnpy" +# RUNTIME_OUTPUT_DIRECTORY_RELEASE "../../../vnpy" +# RUNTIME_OUTPUT_DIRECTORY_DEBUG "../../../vnpy" +# LIBRARY_OUTPUT_DIRECTORY "../../../vnpy") +#elseif(UNIX) +#set_target_properties( +# proglib-cpp PROPERTIES +# RUNTIME_OUTPUT_DIRECTORY "../vnpy" +# RUNTIME_OUTPUT_DIRECTORY_RELEASE "../vnpy" +# RUNTIME_OUTPUT_DIRECTORY_DEBUG "../vnpy" +# LIBRARY_OUTPUT_DIRECTORY "../vnpy") +#endif() + +#if (BUILD_TESTS) +# +# file(GLOB_RECURSE TEST_SOURCE_FILES src/vn/**/*.test.cpp) +# set(TEST_SOURCE_FILES ${TEST_SOURCE_FILES} src/vn/main.cpp) +# +# set(BUILD_SHARED_LIBS ON CACHE BOOL "Build shared libraries (DLLs).") +# add_subdirectory(../libs/googletest gtest) +# +# include_directories(../libs/googletest/include) +# +# add_executable(proglib-cpp-test ${TEST_SOURCE_FILES}) +# +# target_link_libraries(proglib-cpp-test LINK_PUBLIC gtest) +# target_link_libraries(proglib-cpp-test LINK_PUBLIC proglib-cpp) +# +#endif() +# +#if (BUILD_BENCHMARKS) +# +# file(GLOB_RECURSE BENCHMARK_SOURCE_FILES src/vn/**/*.benchmark.cpp) +# set(BENCHMARK_SOURCE_FILES ${BENCHMARK_SOURCE_FILES} src/vn/benchmark.cpp) +# +# add_subdirectory(../libs/hayai hayai) +# add_subdirectory(../libs/glew GLEW_shared) +# +# include_directories(../libs/hayai/src) +# +# add_executable(proglib-cpp-benchmark ${BENCHMARK_SOURCE_FILES}) +# +# #target_link_libraries(proglib-cpp-benchmark LINK_PUBLIC hayai) +# target_link_libraries(proglib-cpp-benchmark LINK_PUBLIC proglib-cpp) +# target_link_libraries(proglib-cpp-benchmark LINK_PUBLIC proglib-cpp-graphics) +# target_link_libraries(proglib-cpp-graphics PRIVATE OpenGL32) +# target_link_libraries(proglib-cpp-graphics PRIVATE GLEW_shared) +# +#endif() diff --git a/ext/vnproglib-1.1.0.115/download.md b/ext/vnproglib-1.1.0.115/download.md new file mode 100644 index 0000000..3b153e4 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/download.md @@ -0,0 +1 @@ +This library was downloaded from http://www.vectornav.com/Downloads/proglib/vnproglib-1.1.0.115.tar.gz \ No newline at end of file diff --git a/ext/vnproglib-1.1.0.115/include/vn/data/error_detection.h b/ext/vnproglib-1.1.0.115/include/vn/data/error_detection.h new file mode 100755 index 0000000..84beef4 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/data/error_detection.h @@ -0,0 +1,55 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifndef _VNDATA_ERROR_DETECTION_H_ +#define _VNDATA_ERROR_DETECTION_H_ + +#include +#include + +#include "vn/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 diff --git a/ext/vnproglib-1.1.0.115/include/vn/event.h b/ext/vnproglib-1.1.0.115/include/vn/event.h new file mode 100755 index 0000000..1f1d755 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/event.h @@ -0,0 +1,290 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifndef _VN_EVENT_H_ +#define _VN_EVENT_H_ + +/*#if _MSC_VER && _WIN32 + #pragma comment(lib, "setupapi.lib") +#endif*/ + +//#include +//#include +#include + +#include "vn/util/export.h" +#include "vn/util/compiler.h" + +#if PYTHON + #include "vn/util/boostpython.h" + #include "vn/python/util.h" +#endif + +namespace vn { + +#if PYTHON + +template +class Event +{ +public: + + Event& operator+=(PyObject* handler) + { + this->add(handler); + + return *this; + } + + // TODO: Prefer the += operator overloaded version. + void add(PyObject* handler) + { + _pycallables.push_back(handler); + handler->ob_refcnt++; + } + + void fire(A1 arg1, A2 arg2, A3 arg3) + //void fire() + { + //packet, index, timestamp + + if (_pycallables.empty()) + return; + + python::AcquireGIL scopedLock; + + #if VN_HAS_RANGE_LOOP + for (auto h : _pycallables) + boost::python::call(h, boost::ref(*this), boost::ref(arg1), arg2, arg3); + #else + for (std::list::iterator it = _pycallables.begin(); it != _pycallables.end(); ++it) + boost::python::call(*it, boost::ref(*this), boost::ref(arg1), arg2, arg3); + #endif + } + + // TODO: Prefer the -= operator overloaded version. + void remove(PyObject* handler) + { + _pycallables.remove(handler); + handler->ob_refcnt--; + } + +private: + + #if defined(_MSC_VER) + #pragma warning(push) + + // Warning about needing dll-interface for _pycallables to be used by clients. + #pragma warning(disable:4251) + #endif + + std::list _pycallables; + + #if defined (_MSC_VER) + #pragma warning(pop) + #endif +}; + +#endif + +#if OLD +/// \brief Provides event like hooks similiar to events in .NET. +/// +/// \remarks This class is intended to provide event functionality in Python +/// while allowing to be accessed from C++. +//template +template +class vn_proglib_DLLEXPORT Event +{ + typedef void(Handler)(Event* caller, A1 arg1); +public: + + //#if PYTHON + + /// \brief Allows Python objects to register with this event. + /// + /// \param[in] handler The Python callable to hook to this event. +// void operator+=(PyObject* handler); + +/// void add(PyObject* handler); + + //{ + // _pycallables.push_back(handler); + // handler->ob_refcnt++; + //} + + //void fire(); +// boost::python::tuple fire(boost::python::tuple args, boost::python::dict kwargs); + + void add(std::function handler) + { + _handlers.push_back(handler); + } + + void fire(A1 arg1) + { + for (auto h : _handlers) + h(this, arg1); + } + + + + //EventT& operator+=(int test) + //void operator+=(int test) + /*void operator+=(PyObject* handler) + { + _pycallables.push_back(handler); + handler->ob_refcnt++; + //_handlers.push_back(test); + + //return *this; + }*/ + + //#endif + + #if NOT_COMPILING_ON_LINUX + + void fire() + { + #if PYTHON + for (auto h : _pycallables) + boost::python::call(h); + #endif + + //vector pRawData(readBuffer, readBuffer + numOfBytesRead); + + //python::AcquireGIL scopedLock; + + //boost::python::call(pi->_rawDataReceivedHandlerPython, pRawData, pi->_dataRunningIndex); + + //for (auto h : _handlers) +// std::cout << h << std::endl; + } + + #endif + +private: + + //std::list _handlers; + + std::list _handlers; + + + #if PYTHON + std::list _pycallables; + #endif +// std::list _handlers; + /*def __init__(self) : + self.__handlers = [] + + def __iadd__(self, handler) : + self.__handlers.append(handler) + return self + + def __isub__(self, handler) : + self.__handlers.remove(handler) + return self + + def fire(self, *args, **keywargs) : + for handler in self.__handlers : + handler(*args, **keywargs) + + def clearObjectHandlers(self, inObject) : + for theHandler in self.__handlers : + if theHandler.im_self == inObject : + self -= theHandler*/ +}; +#endif + +#if NON_TEMPLATED_EVENT + +/// \brief Provides event like hooks similiar to events in .NET. +/// +/// \remarks This class is intended to provide event functionality in Python +/// while allowing to be accessed from C++. +class vn_proglib_DLLEXPORT Event +{ +public: + + //#if PYTHON + + /// \brief Allows Python objects to register with this event. + /// + /// \param[in] handler The Python callable to hook to this event. + void operator+=(PyObject* handler); + + void add(PyObject* handler); + + //{ + // _pycallables.push_back(handler); + // handler->ob_refcnt++; + //} + + //void fire(); + boost::python::tuple fire(boost::python::tuple args, boost::python::dict kwargs); + + + //EventT& operator+=(int test) + //void operator+=(int test) + /*void operator+=(PyObject* handler) + { + _pycallables.push_back(handler); + handler->ob_refcnt++; + //_handlers.push_back(test); + + //return *this; + }*/ + + //#endif + + #if NOT_COMPILING_ON_LINUX + + void fire() + { + #if PYTHON + for (auto h : _pycallables) + boost::python::call(h); + #endif + + //vector pRawData(readBuffer, readBuffer + numOfBytesRead); + + //python::AcquireGIL scopedLock; + + //boost::python::call(pi->_rawDataReceivedHandlerPython, pRawData, pi->_dataRunningIndex); + + //for (auto h : _handlers) +// std::cout << h << std::endl; + } + + #endif + +private: + #if PYTHON + std::list _pycallables; + #endif + std::list _handlers; + /*def __init__(self) : + self.__handlers = [] + + def __iadd__(self, handler) : + self.__handlers.append(handler) + return self + + def __isub__(self, handler) : + self.__handlers.remove(handler) + return self + + def fire(self, *args, **keywargs) : + for handler in self.__handlers : + handler(*args, **keywargs) + + def clearObjectHandlers(self, inObject) : + for theHandler in self.__handlers : + if theHandler.im_self == inObject : + self -= theHandler*/ +}; + +#endif + +} + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/exceptions.h b/ext/vnproglib-1.1.0.115/include/vn/exceptions.h new file mode 100755 index 0000000..33b4688 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/exceptions.h @@ -0,0 +1,104 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +/// \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 + +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: + #if VN_SUPPORTS_CSTR_STRING_CONCATENATE + explicit not_found(std::string itemName) : runtime_error(std::string("Item '" + itemName + "' not found.").c_str()) { } + #else + explicit not_found(std::string itemName) : runtime_error("Item not found.") { } + #endif +}; + +/// \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 diff --git a/ext/vnproglib-1.1.0.115/include/vn/int.h b/ext/vnproglib-1.1.0.115/include/vn/int.h new file mode 100755 index 0000000..4e140a4 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/int.h @@ -0,0 +1,30 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +/// \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 +#endif + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/math/Coordinate2.h b/ext/vnproglib-1.1.0.115/include/vn/math/Coordinate2.h new file mode 100755 index 0000000..bd4742e --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/math/Coordinate2.h @@ -0,0 +1,49 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +/// \file +/// {COMMON_HEADER} +/// +/// \section DESCRIPTION +/// This header file provides the structure Coordinate2. +#ifndef _COORDINATE2_H_ +#define _COORDINATE2_H_ + +#include "vn/int.h" + +namespace vn { +namespace math { + +/// \brief Represents a coordinate in a 2-dimensional space. +template +struct Coordinate2 +{ + // Members //////////////////////////////////////////////////////////////// + +public: + + /// \brief The X component of the coordinate. + T X; + + /// \brief The Y component of the coordinate. + T Y; + + // Constructors /////////////////////////////////////////////////////////// + +public: + + /// \brief Creates a new /ref Coordinate2 with uninitialized X and Y. + Coordinate2() { } + + /// \brief Creates a new /ref Coordinate2 initialized with the provided X + /// and Y values. + /// + /// \param[in] x The initial value for /ref X. + /// \param[in] y The initial value for /ref Y. + Coordinate2(T x, T y) : X(x), Y(y) { } + +}; + +} +} + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/math/attitude.h b/ext/vnproglib-1.1.0.115/include/vn/math/attitude.h new file mode 100755 index 0000000..fb725db --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/math/attitude.h @@ -0,0 +1,85 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifndef _VNMATH_ATTITUDE_H_ +#define _VNMATH_ATTITUDE_H_ + +#include "vector.h" +#include "matrix.h" +#include "vn/util/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 AttitudeF representing no rotation. + static AttitudeF noRotation(); + + /// \brief Creates a new AttitudeF from a quaternion. + /// + /// \param[in] quat The orientation expressed as a quaternion. + /// \return The new AttitudeF. + static AttitudeF fromQuat(vec4f quat); + + /// \brief Creates a new AttitudeF from a yaw, pitch, roll in degrees. + /// + /// \param[in] yprInDegs The yaw, pitch, roll in degrees. + /// \return The new AttitudeF. + static AttitudeF fromYprInDegs(vec3f yprInDegs); + + /// \brief Creates a new AttitudeF from a yaw, pitch, roll in radians. + /// + /// \param[in] yprInRads The yaw, pitch, roll in radians. + /// \return The new AttitudeF. + static AttitudeF fromYprInRads(vec3f yprInRads); + + /// \brief Creates a new AttitudeF from a direction cosine matrix. + /// + /// \param[in] dcm The direction cosine matrix. + /// \return The new AttitudeF. + static AttitudeF fromDcm(mat3f dcm); + +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 diff --git a/ext/vnproglib-1.1.0.115/include/vn/math/consts.h b/ext/vnproglib-1.1.0.115/include/vn/math/consts.h new file mode 100755 index 0000000..39ef66f --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/math/consts.h @@ -0,0 +1,50 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +/// \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 diff --git a/ext/vnproglib-1.1.0.115/include/vn/math/conversions.h b/ext/vnproglib-1.1.0.115/include/vn/math/conversions.h new file mode 100755 index 0000000..e59171e --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/math/conversions.h @@ -0,0 +1,248 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifndef _VN_MATH_CONVERSIONS_H_ +#define _VN_MATH_CONVERSIONS_H_ + +#include "vn/util/export.h" +#include "vn/math/vector.h" +#include "vn/math/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 +vec rad2deg(vec 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 +//vec vn_proglib_DLLEXPORT deg2rad(vec anglesInDegs) +template +vec deg2rad(vec 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); + +} +} + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/math/kinematics.h b/ext/vnproglib-1.1.0.115/include/vn/math/kinematics.h new file mode 100755 index 0000000..9745f94 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/math/kinematics.h @@ -0,0 +1,262 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +/// \file +/// {COMMON_HEADER} +/// +/// \section DESCRIPTION +/// This header file provides the structure quat. +#ifndef _VN_KINAMATICS_H_ +#define _VN_KINAMATICS_H_ + +namespace vn { +namespace math { +namespace kinematics { + +/// \brief Represents orientation in yaw, pitch, roll. +template +struct ypr +{ + + // Members //////////////////////////////////////////////////////////////// + +public: + + /// \brief The yaw value in radians. + T yaw; + + /// \brief The pitch value in radians. + T pitch; + + /// \brief The roll value in radians. + T roll; + + // Constructors /////////////////////////////////////////////////////////// + +public: + + /// \brief Creates a new uninitialized \ref ypr. + ypr() { } + + /// \brief Creates a \ref ypr initialized to the provided values. + /// + /// \param[in] yawInRads The initial \ref yaw value in radians. + /// \param[in] pitchInRads The initial \ref pitch value in radians. + /// \param[in] rollInRads The initial \ref roll value in radians. + ypr(T yawInRads, T pitchInRads, T rollInRads) : + yaw(yawInRads), + pitch(pitchInRads), + roll(rollInRads) { } + + // Helper Methods ///////////////////////////////////////////////////////// + + /// \brief Returns a \ref ypr initialized with the provided values + /// in degrees. + /// + /// \param[in] yawInDegs The yaw value in degrees. + /// \param[in] pitchInDegs The pitch value in degrees. + /// \param[in] rollInDegs The roll value in degrees. + /// \return The new \ref ypr initialized from the provided values. + static ypr fromDegs( + T yawInDegs, + T pitchInDegs, + T rollInDegs) + { + return ypr( + vn::math::deg2rad(yawInDegs), + vn::math::deg2rad(pitchInDegs), + vn::math::deg2rad(rollInDegs)); + } + + /// \brief Returns a \ref ypr representing no rotation. + /// + /// \return The \ref ypr representing no rotation. + static ypr noRotation() + { + return ypr(0, 0, 0); + } + + // Public Methods ///////////////////////////////////////////////////////// + +public: + + /// \brief Returns the \ref yaw value in degrees. + /// + /// \return The \ref yaw value in degrees. + T yawDegs() const + { + return math::rad2deg(Yyaw); + } + + /// \brief Returns the \ref pitch value in degrees. + /// + /// \return The \ref pitch value in degrees. + T pitchDegs() const + { + return math::rad2deg(pitch); + } + + /// \brief Returns the \ref roll value in degrees. + /// + /// \return The \ref roll value in degrees. + T rollDegs() const + { + return math::rad2deg(roll); + } + + /// \brief Sets the \ref yaw to the value provided in degrees. + /// + /// \param[in] yawInDegs The \ref yaw value in degrees. + void setYawDegs(T yawInDegs) + { + yaw = math::deg2rad(yawInDegs); + } + + /// \brief Sets the \ref pitch to the value provided in degrees. + /// + /// \param[in] pitchInDegs The \ref pitch value in degrees. + void setPitchDegs(T pitchInDegs) + { + pitch = math::deg2rad(pitchInDegs); + } + + /// \brief Sets the \ref roll to the value provided in degrees. + /// + /// \param[in] rollInDegs The \ref roll value in degrees. + void setRollDegs(T rollInDegs) + { + roll = math::deg2rad(rollInDegs); + } + + /// \brief Returns the \ref YawPitchRoll as a + /// \ref vn::math::linearalgebra::Vector3. + /// + /// \return The \ref YawPitchRoll as a + /// \ref vn::math::linearalgebra::Vector3 with the components in + /// radians. +/* vn::math::linearalgebra::Vector3 ToVector3() const + { + return vn::math::linearalgebra::Vector3(Yaw, Pitch, Roll); + }*/ + + /// \brief Returns the \ref YawPitchRoll as a + /// \ref vn::math::linearalgebra::Vector3. + /// + /// \return The \ref YawPitchRoll as a + /// \ref vn::math::linearalgebra::Vector3 with the components in + /// degrees. +/* vn::math::linearalgebra::Vector3 ToVector3InDegs() const + { + return vn::math::linearalgebra::Vector3( + MConv::RadsToDegs(Yaw), + MConv::RadsToDegs(Pitch), + MConv::RadsToDegs(Roll)); + }*/ +}; + +// Specific Typedefs ////////////////////////////////////////////////////////// + +/// \brief Data type representing a \ref ypr using float as the +/// underlying data type. +typedef ypr yprf; + +/// \brief Data type representing a \ref ypr using double as +/// the underlying data type. +typedef ypr yprd; + +/// \brief Data type representing a \ref ypr using long double +/// as the underlying data type. +typedef ypr yprld; + +/// \brief Represents orientation as a quaternion. +template +struct quat +{ + + // Members //////////////////////////////////////////////////////////////// + +public: + + /// \brief The x value. + T x; + + /// \brief The y value. + T y; + + /// \brief The z value. + T z; + + /// \brief The w value. + T w; + + // Constructors /////////////////////////////////////////////////////////// + +public: + + /// \brief Creates a new uninitialized \ref quat. + quat() { } + + /// \brief Creates a \ref quat initialized to the provided values. + /// + /// \param[in] x The initial \ref x value. + /// \param[in] y The initial \ref y value. + /// \param[in] z The initial \ref z value. + /// \param[in] w The initial \ref w value. + quat(T xIn, T yIn, T zIn, T wIn) : + x(xIn), + y(yIn), + z(zIn), + w(wIn) + { } + + // Helper Methods ///////////////////////////////////////////////////////// + +public: + +#if 0 + /// \brief Returns a \ref YawPitchRoll from the values in degrees. + /// + /// \param[in] yawInDegs The yaw value in degrees. + /// \param[in] pitchInDegs The pitch value in degrees. + /// \param[in] rollInDegs The roll value in degrees. + /// \return The new \ref YawPitchRoll initialized from the provided values. + static YawPitchRoll CreateFromDegs( + T yawInDegs, + T pitchInDegs, + T rollInDegs) + { + return YawPitchRoll( + vn::math::MConv::DegsToRads(yawInDegs), + vn::math::MConv::DegsToRads(pitchInDegs), + vn::math::MConv::DegsToRads(rollInDegs)); + } +#endif + + /// \brief Returns a \ref quat representing no rotation. + /// + /// \return The \ref quat representing no rotation. + static quat noRotation() + { + return quat(0, 0, 0, 1); + } + +}; + +// Specific Typedefs ////////////////////////////////////////////////////////// + +/// \brief Data type representing a \ref quat using float as the +/// underlying data type. +typedef quat quatf; + +/// \brief Data type representing a \ref quat using double as +/// the underlying data type. +typedef quat quatd; + +/// \brief Data type representing a \ref quat using long double +/// as the underlying data type. +typedef quat quatld; + +} +} +} + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/math/math.h b/ext/vnproglib-1.1.0.115/include/vn/math/math.h new file mode 100755 index 0000000..fd29a10 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/math/math.h @@ -0,0 +1,16 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +/// \file +/// {COMMON_HEADER} +/// +/// \section DESCRIPTION +/// This header file pulls in all of the header files for the VectorNav C++ +/// Math Library. +#ifndef _VN_MATH_MATH_H_ +#define _VN_MATH_MATH_H_ + +#include "vn/math/consts.h" +#include "vn/math/vector.h" +#include "vn/math/matrix.h" + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/math/matrix.h b/ext/vnproglib-1.1.0.115/include/vn/math/matrix.h new file mode 100755 index 0000000..3e85f70 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/math/matrix.h @@ -0,0 +1,1679 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +/// \file +/// {COMMON_HEADER} +/// +/// \section DESCRIPTION +/// This header file provides may types for working with matrices. +#ifndef _VN_MATH_MAT_H_ +#define _VN_MATH_MAT_H_ + +#include + +#include "vn/math/vector.h" +#include "vn/exceptions.h" +#include + +namespace vn { +namespace math { + +/// \brief Template for a matrix. +template +struct mat +{ + // Public Members ///////////////////////////////////////////////////////// + +public: + + /// \brief The matrix's elements. + union + { + T e[m * n]; + }; + + // Constructors /////////////////////////////////////////////////////////// + +public: + + /// \brief Creates a new matrix with uninitialized elements. + mat() { } + + /// \brief Creates a new matrix with ints elements initialized to val. + /// + /// \param[in] val The initialization value. + explicit mat(T val) + { + std::fill_n(e, m * n, val); + } + + // Helper Methods ///////////////////////////////////////////////////////// + +public: + + /// \brief Matrix with all of its elements set to 0. + /// + /// \return The 0 matrix. + static mat zero() + { + return mat(0); + } + + /// \brief Matrix with all of its elements set to 1. + /// + /// \return The 1 matrix. + static mat one() + { + return mat(1); + } + + /// \brief Identity matrix with its diagonal elements set to 1. + /// + /// \return The identity matrix. + static mat identity() + { + assert(m == n); + + mat nm(0); + + for (size_t i = 0; i < m; i++) + nm.e[i * m + i] = 1; + + return nm; + } + + // Operator Overloads ///////////////////////////////////////////////////// + +public: + + /// \brief Indexing into the matrix's elements. + /// + /// \param[in] row The 0-based index row. + /// \param[in] col The 0-based index column. + /// \return The requested element. + T& operator()(size_t row, size_t col) + { + return const_cast(static_cast(*this)(row, col)); + } + + /// \brief Indexing into the matrix's elements. + /// + /// \param[in] row The 0-based index row. + /// \param[in] col The 0-based index column. + /// \return The requested element. + const T& operator()(size_t row, size_t col) const + { + assert(row < m && col < n); + + //return e[col * m + row]; + return e[col + row * m]; + } + + /// \brief Negates the matrix. + /// + /// \return The negated matrix. + mat operator-() const + { + return neg(); + } + + /// \brief Multiplies the matrix by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The multiplied matrix. + template + mat& operator*=(const S& rhs) + { + for (size_t i = 0; i < m*n; i++) + e[i] *= rhs; + + return *this; + } + + /// \brief Multiplies the matrix by another matrix + /// + /// \param[in] rhs The other matrix. + /// \return The multiplied matrix. + template + mat& operator*(const mat& rhs) + { + // columns from the matrix must match rows of the input matrix + if (m != s) + { + return *this; + } + + size_t row = 0; + size_t col = 0; + size_t cell = 0; + + mat return_mat = zero(); + + // Remember, this matrix is stored in column order + for (size_t row_index = 0; row_index < r; row_index++) + { + for (size_t col_index = 0; col_index < n; col_index++) + { + + cell = row_index + (col_index * r); + + for (size_t cell_index = 0; cell_index < m; cell_index++) + { + // calculated for the cell in the current row + row = (col_index * m) + cell_index; + // calculated for the cell in the current column + col = (cell_index * r) + row_index; + return_mat.e[cell] += this->e[row] * rhs.e[col]; + } + } + } + + return return_mat; + } + + /// \brief Divides the matrix by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The divided matrix. + template + mat& operator/=(const T & rhs) + { + for (size_t i = 0; i < m*n; i++) + e[i] /= rhs; + + return *this; + } + + /// \brief Adds a matrix to this matrix. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + template + mat& operator+=(const mat& rhs) + { + for (size_t i = 0; i < m*n; i++) + e[i] += rhs.e[i]; + + return *this; + } + + /// \brief Subtracts a matrix from this matrix. + /// + /// \param[in] rhs The right-side matrix. + /// \return The resulting matrix. + template + mat& operator-=(const mat& rhs) + { + for (size_t i = 0; i < m*n; i++) + e[i] -= rhs.e[i]; + + return *this; + } + + // Public Methods ///////////////////////////////////////////////////////// + +public: + + /// \brief The matrix's row dimension. + /// + /// \return The matrix's row dimension. + size_t dimRow() const { return m; } + + /// \brief The matrix's column dimension. + /// + /// \return The matrix's column dimension. + size_t dimCol() const { return n; } + + /// \brief Negates the matrix. + /// + /// \return The negated matrix. + mat neg() const + { + mat nm; + + for (size_t i = 0; i < m * n; i++) + nm.e[i] = -e[i]; + + return nm; + } + + /// \brief Multiplies the matrix by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The multiplied matrix. + mat mult(const double& scalar) const + { + mat nm; + + for (size_t i = 0; i < m*n; i++) + nm.e[i] = e[i] * scalar; + + return nm; + } + + /// \brief Divides the matrix by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The divided matrix. + mat div(const double& scalar) const + { + mat nm; + + for (size_t i = 0; i < m*n; i++) + nm.e[i] = e[i] / scalar; + + return nm; + } + + /// \brief Adds a matrix to this matrix. + /// + /// \param[in] toAdd The matrix to add. + /// \return The resulting matrix. + mat add(const mat& toAdd) const + { + mat nm; + + for (size_t i = 0; i < m*n; i++) + nm.e[i] = e[i] + toAdd.e[i]; + + return nm; + } + + /// \brief Subtracts a matrix from this matrix. + /// + /// \param[in] toSub The matrix to subtract from this. + /// \return The resulting matrix. + mat sub(const mat& toSub) const + { + mat nm; + + for (size_t i = 0; i < m*n; i++) + nm.e[i] = e[i] - toSub.e[i]; + + return nm; + } + + /// \brief Transposes the matrix. + /// + /// \return The computed transpose. + mat transpose() const + { + mat nm; + + for (size_t row = 0; row < m; row++) + { + for (size_t col = 0; col < n; col++) + { + nm.e[row * n + col] = e[col * m + row]; + } + } + + return nm; + } + +}; + +// Specializations //////////////////////////////////////////////////////////// + +#if defined(_MSC_VER) + #pragma warning(push) + + // Disable warning about 'nonstandard extension used : nameless struct/union'. + #pragma warning(disable:4201) +#endif + +/// \brief 2x2 matrix specialization. +template +struct mat<2, 2, T> +{ + + // Public Members ///////////////////////////////////////////////////////// + +public: + + union + { + struct + { + /// \brief Element 0,0. + T e00; + + /// \brief Element 1,0. + T e10; + + /// \brief Element 0,1. + T e01; + + /// \brief Element 1,1. + T e11; + }; + + /// \brief The matrix's elements. + T e[2 * 2]; + }; + + // Constructors /////////////////////////////////////////////////////////// + +public: + + /// \brief Creates a new matrix with uninitialized elements. + mat() { } + + /// \brief Creates a new matrix with its elements initialized to val. + /// + /// \param[in] val The initialization value. + explicit mat(T val) : + e00(val), e10(val), + e01(val), e11(val) + { } + + /// \brief Creates a new matrix with its components initialized to the + /// provided values. + /// + /// \param[in] e00v Element 0,0 value. + /// \param[in] e01v Element 0,1 value. + /// \param[in] e10v Element 1,0 value. + /// \param[in] e11v Element 1,1 value. + mat(T e00v, T e01v, + T e10v, T e11v) : + e00(e00v), e10(e10v), + e01(e01v), e11(e11v) + { } + + /// \brief Constructs a matrix from 4 column vectors. + /// + /// \param[in] col0 The 0 column vector. + /// \param[in] col1 The 1 column vector. + /// \param[in] col2 The 2 column vector. + /// \param[in] col3 The 3 column vector. + mat(vec<2, T> col0, vec<2, T> col1) : + e00(col0.x), e10(col1.x), + e01(col0.y), e11(col1.y) + {} + + // Helper Methods ///////////////////////////////////////////////////////// + +public: + + /// \brief Matrix with all of its elements set to 0. + /// + /// \return The 0 matrix. + static mat zero() + { + return mat<2, 2, T>(0); + } + + /// \brief Matrix with all of its elements set to 1. + /// + /// \return The 1 matrix. + static mat one() + { + return mat<2, 2, T>(1); + } + + /// \brief Identity matrix with its diagonal elements set to 1. + /// + /// \return The identity matrix. + static mat<2, 2, T> identity() + { + return mat<2, 2, T>( + 1, 0, + 0, 1); + } + + // Operator Overloads ///////////////////////////////////////////////////// + +public: + + /// \brief Indexing into the matrix's elements. + /// + /// \param[in] row The 0-based index row. + /// \param[in] col The 0-based index column. + /// \return The requested element. + T& operator()(size_t row, size_t col) + { + return const_cast(static_cast(*this)(row, col)); + } + + /// \brief Indexing into the matrix's elements. + /// + /// \param[in] row The 0-based index row. + /// \param[in] col The 0-based index column. + /// \return The requested element. + const T& operator()(size_t row, size_t col) const + { + assert(row < 2 && col < 2); + + return e[col * 2 + row]; + } + + /// \brief Negates the matrix. + /// + /// \return The negated matrix. + mat operator-() const + { + return neg(); + } + + /// \brief Multiplies the matrix by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The multiplied matrix. + template + mat& operator*=(const S& rhs) + { + #if defined(_MSC_VER) + #pragma warning(push) + + // The operator*= throws a warning when a mat*f is multiplied by a mat*d. + #pragma warning(disable:4244) + #endif + + for (size_t i = 0; i < 2 * 2; i++) + e[i] *= rhs; + + #if defined (_MSC_VER) + #pragma warning(pop) + #endif + + return *this; + } + + /// \brief Multiplies the matrix by another matrix. + /// + /// \param[in] rhs The other matrix. + /// \return The multiplied matrix. + template + mat& operator*=(const mat<2, 2, S>& rhs) + { + size_t row = 0; + size_t col = 0; + size_t cell = 0; + + mat<2, 2, T> return_mat = zero(); + + // Remember, this matrix is stored in column order + for (size_t row_index = 0; row_index < 2; row_index++) + { + for (size_t col_index = 0; col_index < 2; col_index++) + { + cell = row_index + (col_index * 2); + + for (size_t cell_index = 0; cell_index < 2; cell_index++) + { + // calculated for the cell in the current row + row = row_index + (cell_index * 2); + // calculated for the cell in the current column + col = cell_index + (col_index * 2); + + return_mat.e[cell] += this->e[row] * rhs.e[col]; + } + } + } + + *this = return_mat; + + return *this; + } + + /// \brief Divides the matrix by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The divided matrix. + template + mat& operator/=(const S& rhs) + { + #if defined(_MSC_VER) + #pragma warning(push) + + // The operator/= throws a warning when a mat*f is divided by a double. + #pragma warning(disable:4244) + #endif + + for (size_t i = 0; i < 2 * 2; i++) + e[i] /= rhs; + + #if defined (_MSC_VER) + #pragma warning(pop) + #endif + + return *this; + } + + /// \brief Adds a matrix to this matrix. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + template + mat& operator+=(const mat<2, 2, S>& rhs) + { + for (size_t i = 0; i < 2 * 2; i++) + e[i] += rhs.e[i]; + + return *this; + } + + /// \brief Subtracts a matrix from this matrix. + /// + /// \param[in] rhs The right-side matrix. + /// \return The resulting matrix. + template + mat& operator-=(const mat<2, 2, S>& rhs) + { + for (size_t i = 0; i < 2 * 2; i++) + e[i] -= rhs.e[i]; + + return *this; + } + + // Public Methods ///////////////////////////////////////////////////////// + +public: + + /// \brief The matrix's row dimension. + /// + /// \return The matrix's row dimension. + size_t dimRow() const { return 2; } + + /// \brief The matrix's column dimension. + /// + /// \return The matrix's column dimension. + + size_t dimCol() const { return 2; } + + template + void copy(const mat<2, 2, S> rhs) + { + *this = rhs; + } + + /// \brief Negates the matrix. + /// + /// \return The negated matrix. + mat neg() const + { + mat nm; + + for (size_t i = 0; i < 2 * 2; i++) + nm.e[i] = -e[i]; + + return nm; + } + + /// \brief Multiplies the matrix by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The multiplied matrix. + mat mult(const double& scalar) const + { + mat nm; + + for (size_t i = 0; i < 2 * 2; i++) + nm.e[i] = e[i] * scalar; + + return nm; + } + + /// \brief Divides the matrix by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The divided matrix. + mat div(const double& scalar) const + { + mat nm; + + for (size_t i = 0; i < 2 * 2; i++) + nm.e[i] = e[i] / scalar; + + return nm; + } + + /// \brief Adds a matrix to this matrix. + /// + /// \param[in] toAdd The matrix to add. + /// \return The resulting matrix. + mat add(const mat& toAdd) const + { + mat nm; + + for (size_t i = 0; i < 2 * 2; i++) + nm.e[i] = e[i] + toAdd.e[i]; + + return nm; + } + + /// \brief Subtracts a matrix from this matrix. + /// + /// \param[in] toSub The matrix to subtract from this. + /// \return The resulting matrix. + mat sub(const mat& toSub) const + { + mat nm; + + for (size_t i = 0; i < 2 * 2; i++) + nm.e[i] = e[i] - toSub.e[i]; + + return nm; + } + + /// \brief Transposes the matrix. + /// + /// \return The computed transpose. + mat<2, 2, T> transpose() const + { + mat<2, 2, T> nm; + + for (size_t row = 0; row < 2; row++) + { + for (size_t col = 0; col < 2; col++) + { + nm.e[row * 2 + col] = e[col * 2 + row]; + } + } + + return nm; + } +}; + +/// \brief 3x3 matrix specialization. +/// \brief matrix is column ordered in memory +template +struct mat<3, 3, T> +{ + + // Public Members ///////////////////////////////////////////////////////// + +public: + + union + { + struct + { + /// \brief Element 0,0. + T e00; + + /// \brief Element 1,0. + T e10; + + /// \brief Element 2,0. + T e20; + + /// \brief Element 0,1. + T e01; + + /// \brief Element 1,1. + T e11; + + /// \brief Element 2,1. + T e21; + + /// \brief Element 0,2. + T e02; + + /// \brief Element 1,2. + T e12; + + /// \brief Element 2,2. + T e22; + }; + + /// \brief The matrix's elements. + T e[3 * 3]; + }; + + // Constructors /////////////////////////////////////////////////////////// + +public: + + /// \brief Creates a new matrix with uninitialized elements. + mat() { } + + /// \brief Creates a new matrix with its elements initialized to val. + /// + /// \param[in] val The initialization value. + explicit mat(T val) : + e00(val), e10(val), e20(val), + e01(val), e11(val), e21(val), + e02(val), e12(val), e22(val) + { } + + /// \brief Creates a new matrix with its components intialized to the + /// provided values. + /// + /// \param[in] e00v Element 0,0 value. + /// \param[in] e01v Element 0,1 value. + /// \param[in] e02v Element 0,2 value. + /// \param[in] e10v Element 1,0 value. + /// \param[in] e11v Element 1,1 value. + /// \param[in] e12v Element 1,2 value. + /// \param[in] e20v Element 2,0 value. + /// \param[in] e21v Element 2,1 value. + /// \param[in] e22v Element 2,2 value. + mat(T e00v, T e01v, T e02v, + T e10v, T e11v, T e12v, + T e20v, T e21v, T e22v) : + e00(e00v), e10(e10v), e20(e20v), + e01(e01v), e11(e11v), e21(e21v), + e02(e02v), e12(e12v), e22(e22v) + + { } + + /// \brief Constructs a matrix from 3 column vectors. Vectors are stored in column order + /// + /// \param[in] col0 The 0 column vector. + /// \param[in] col1 The 1 column vector. + /// \param[in] col2 The 2 column vector. + mat(vec<3, T> col0, vec<3, T> col1, vec<3, T> col2) : + e00(col0.x), e10(col0.y), e20(col0.z), + e01(col1.x), e11(col1.y), e21(col1.z), + e02(col2.x), e12(col2.y), e22(col2.z) + { + + } + + // Helper Methods ///////////////////////////////////////////////////////// + +public: + + /// \brief Matrix with all of its elements set to 0. + /// + /// \return The 0 matrix. + static mat zero() + { + return mat<3, 3, T>(0); + } + + /// \brief Matrix with all of its elements set to 1. + /// + /// \return The 1 matrix. + static mat one() + { + return mat<3, 3, T>(1); + } + + /// \brief Identity matrix with its diagonal elements set to 1. + /// + /// \return The identity matrix. + static mat<3, 3, T> identity() + { + return mat<3, 3, T>( + 1, 0, 0, + 0, 1, 0, + 0, 0, 1); + } + + // Operator Overloads ///////////////////////////////////////////////////// + +public: + + /// \brief Indexing into the matrix's elements. + /// + /// \param[in] row The 0-based index row. + /// \param[in] col The 0-based index column. + /// \return The requested element. + T& operator()(size_t row, size_t col) + { + return const_cast(static_cast(*this)(row, col)); + } + + /// \brief Indexing into the matrix's elements. + /// + /// \param[in] row The 0-based index row. + /// \param[in] col The 0-based index column. + /// \return The requested element. + const T& operator()(size_t row, size_t col) const + { + assert(row < 3 && col < 3); + + return e[col * 3 + row]; + } + + /// \brief Negates the matrix. + /// + /// \return The negated matrix. + mat operator-() const + { + return neg(); + } + + /// \brief Multiplies the matrix by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The multiplied matrix. + template + mat& operator*=(const S& rhs) + { + #if defined(_MSC_VER) + #pragma warning(push) + + // The operator*= throws a warning when a mat*f is multiplied by a mat*d. + #pragma warning(disable:4244) + #endif + + for (size_t i = 0; i < 3 * 3; i++) + e[i] *= rhs; + + #if defined (_MSC_VER) + #pragma warning(pop) + #endif + + return *this; + } + + /// \brief Multiplies the matrix by another matrix. + /// + /// \param[in] rhs The other matrix. + /// \return The multiplied matrix. + template + mat& operator*=(const mat<3, 3, S>& rhs) + { + size_t counter = 0; + size_t row = 0; + size_t col = 0; + size_t cell = 0; + mat<3, 3, T> return_mat = zero(); + + // Remember, this matrix is stored in column order + for(size_t row_index = 0; row_index < 3; row_index++) + { + for(size_t col_index = 0; col_index < 3; col_index++) + { + cell = row_index + (col_index * 3); + + for (size_t cell_index = 0; cell_index < 3; cell_index++) + { + // calculated for the cell in the current row + row = row_index + (cell_index * 3); + // calculated for the cell in the current column + col = cell_index + (col_index * 3); + + return_mat.e[cell] += this->e[row] * rhs.e[col]; + } + } + } + + // std::copy(return_mat.e[0], return_mat.e[8], this->e[0]); + // std::copy(return_mat.e, return_mat.e + 7, this->e); + *this = return_mat; + + return *this; + } + + /// \brief Divides the matrix by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The divided matrix. + template + mat& operator/=(const S & rhs) + { + #if defined(_MSC_VER) + #pragma warning(push) + + // The operator/= throws a warning when a mat*f is divided by a double. + #pragma warning(disable:4244) + #endif + + for (size_t i = 0; i < 3 * 3; i++) + e[i] /= rhs; + + #if defined (_MSC_VER) + #pragma warning(pop) + #endif + + return *this; + } + + /// \brief Adds a matrix to this matrix. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + template + mat& operator+=(const mat<3, 3, S>& rhs) + { + for (size_t i = 0; i < 3 * 3; i++) + e[i] += rhs.e[i]; + + return *this; + } + + /// \brief Subtracts a matrix from this matrix. + /// + /// \param[in] rhs The right-side matrix. + /// \return The resulting matrix. + template + mat& operator-=(const mat<3, 3, S>& rhs) + { + for (size_t i = 0; i < 3 * 3; i++) + e[i] -= rhs.e[i]; + + return *this; + } + + // Public Methods ///////////////////////////////////////////////////////// + +public: + + /// \brief The matrix's row dimension. + /// + /// \return The matrix's row dimension. + size_t dimRow() const { return 3; } + + /// \brief The matrix's column dimension. + /// + /// \return The matrix's column dimension. + size_t dimCol() const { return 3; } + size_t dimCols() const { return 3; } + + template + void copy(const mat<3, 3, S>& rhs) + { + *this = rhs; + } + + /// \brief Negates the matrix. + /// + /// \return The negated matrix. + mat neg() const + { + mat nm; + + for (size_t i = 0; i < 3 * 3; i++) + nm.e[i] = -e[i]; + + return nm; + } + + /// \brief Multiplies the matrix by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The multiplied matrix. + mat mult(const double& scalar) const + { + mat nm; + + for (size_t i = 0; i < 3 * 3; i++) + nm.e[i] = e[i] * scalar; + + return nm; + } + + /// \brief Divides the matrix by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The divided matrix. + mat div(const double& scalar) const + { + mat nm; + + for (size_t i = 0; i < 3 * 3; i++) + nm.e[i] = e[i] / scalar; + + return nm; + } + + /// \brief Adds a matrix to this matrix. + /// + /// \param[in] toAdd The matrix to add. + /// \return The resulting matrix. + mat add(const mat& toAdd) const + { + mat nm; + + for (size_t i = 0; i < 3 * 3; i++) + nm.e[i] = e[i] + toAdd.e[i]; + + return nm; + } + + /// \brief Subtracts a matrix from this matrix. + /// + /// \param[in] toSub The matrix to subtract from this. + /// \return The resulting matrix. + mat sub(const mat& toSub) const + { + mat nm; + + for (size_t i = 0; i < 3 * 3; i++) + nm.e[i] = e[i] - toSub.e[i]; + + return nm; + } + + /// \brief Transposes the matrix. + /// + /// \return The computed transpose. + mat<3, 3, T> transpose() const + { + mat<3, 3, T> nm; + + for (size_t row = 0; row < 3; row++) + { + for (size_t col = 0; col < 3; col++) + { + nm.e[row * 3 + col] = e[col * 3 + row]; + } + } + + return nm; + } +}; + +/// \brief 4x4 matrix specialization. +template +struct mat<4, 4, T> +{ + + // Public Members ///////////////////////////////////////////////////////// + +public: + + union + { + struct + { + /// \brief Element 0,0. + T e00; + + /// \brief Element 1,0. + T e10; + + /// \brief Element 2,0. + T e20; + + /// \brief Element 3,0. + T e30; + + /// \brief Element 0,1. + T e01; + + /// \brief Element 1,1. + T e11; + + /// \brief Element 2,1. + T e21; + + /// \brief Element 3,1. + T e31; + + /// \brief Element 0,2. + T e02; + + /// \brief Element 1,2. + T e12; + + /// \brief Element 2,2. + T e22; + + /// \brief Element 3,2. + T e32; + + /// \brief Element 0,3. + T e03; + + /// \brief Element 1,3. + T e13; + + /// \brief Element 2,3. + T e23; + + /// \brief Element 3,3. + T e33; + }; + + /// \brief The matrix's elements. + T e[4*4]; + }; + + // Constructors /////////////////////////////////////////////////////////// + +public: + + /// \brief Creates a new matrix with uninitialized elements. + mat() { } + + /// \brief Creates a new matrix with its elements initialized to val. + /// + /// \param[in] val The initialization value. + explicit mat(T val) : + e00(val), e01(val), e02(val), e03(val), + e10(val), e11(val), e12(val), e13(val), + e20(val), e21(val), e22(val), e23(val), + e30(val), e31(val), e32(val), e33(val) + { } + + /// \brief Creates a new matrix with its components intialized to the + /// provided values. + /// + /// \param[in] e00v Element 0,0 value. + /// \param[in] e01v Element 0,1 value. + /// \param[in] e02v Element 0,2 value. + /// \param[in] e03v Element 0,3 value. + /// \param[in] e10v Element 1,0 value. + /// \param[in] e11v Element 1,1 value. + /// \param[in] e12v Element 1,2 value. + /// \param[in] e13v Element 1,3 value. + /// \param[in] e20v Element 2,0 value. + /// \param[in] e21v Element 2,1 value. + /// \param[in] e22v Element 2,2 value. + /// \param[in] e23v Element 2,3 value. + /// \param[in] e30v Element 3,0 value. + /// \param[in] e31v Element 3,1 value. + /// \param[in] e32v Element 3,2 value. + /// \param[in] e33v Element 3,3 value. + mat(T e00v, T e01v, T e02v, T e03v, + T e10v, T e11v, T e12v, T e13v, + T e20v, T e21v, T e22v, T e23v, + T e30v, T e31v, T e32v, T e33v) : + e00(e00v), e01(e01v), e02(e02v), e03(e03v), + e10(e10v), e11(e11v), e12(e12v), e13(e13v), + e20(e20v), e21(e21v), e22(e22v), e23(e23v), + e30(e30v), e31(e31v), e32(e32v), e33(e33v) + { } + + /// \brief Constructs a matrix from 4 column vectors. + /// + /// \param[in] col0 The 0 column vector. + /// \param[in] col1 The 1 column vector. + /// \param[in] col2 The 2 column vector. + /// \param[in] col3 The 3 column vector. + mat(vec<4, T> col0, vec<4, T> col1, vec<4, T> col2, vec<4, T> col3) : + e00(col0.x), e10(col1.x), e20(col2.x), e30(col3.x), + e01(col0.y), e11(col1.y), e21(col2.y), e31(col3.y), + e02(col0.z), e12(col1.z), e22(col2.z), e32(col3.z), + e03(col0.w), e13(col1.w), e23(col2.w), e33(col3.w) + { + + } + + // Helper Methods ///////////////////////////////////////////////////////// + +public: + + /// \brief Matrix with all of its elements set to 0. + /// + /// \return The 0 matrix. + static mat zero() + { + return mat<4, 4, T>(0); + } + + /// \brief Matrix with all of its elements set to 1. + /// + /// \return The 1 matrix. + static mat one() + { + return mat<4, 4, T>(1); + } + + /// \brief Identity matrix with its diagonal elements set to 1. + /// + /// \return The identity matrix. + static mat<4, 4, T> identity() + { + return mat<4, 4, T>(1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1); + } + + // Operator Overloads ///////////////////////////////////////////////////// + +public: + + /// \brief Indexing into the matrix's elements. + /// + /// \param[in] row The 0-based index row. + /// \param[in] col The 0-based index column. + /// \return The requested element. + T& operator()(size_t row, size_t col) + { + return const_cast(static_cast(*this)(row, col)); + } + + /// \brief Indexing into the matrix's elements. + /// + /// \param[in] row The 0-based index row. + /// \param[in] col The 0-based index column. + /// \return The requested element. + const T& operator()(size_t row, size_t col) const + { + assert(row < 4 && col < 4); + + return e[col * 4 + row]; + } + + /// \brief Negates the matrix. + /// + /// \return The negated matrix. + mat operator-() const + { + return neg(); + } + + /// \brief Multiplies the matrix by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The multiplied matrix. + template + mat& operator*=(const S& rhs) + { + #if defined(_MSC_VER) + #pragma warning(push) + + // The operator*= throws a warning when a mat*f is multiplied by a mat*d. + #pragma warning(disable:4244) + #endif + + for (size_t i = 0; i < 4 * 4; i++) + e[i] *= rhs; + + #if defined (_MSC_VER) + #pragma warning(pop) + #endif + + return *this; + } + + /// \brief Multiplies the matrix by another matrix. + /// + /// \param[in] rhs The other matrix. + /// \return The multiplied matrix. + template + mat& operator*=(const mat<4, 4, S>& rhs) + { + size_t row = 0; + size_t col = 0; + size_t cell = 0; + + mat<4, 4, T> return_mat = zero(); + + // Remember, this matrix is stored in column order + for (size_t row_index = 0; row_index < 4; row_index++) + { + for (size_t col_index = 0; col_index < 4; col_index++) + { + cell = row_index + (col_index * 4); + + for (size_t cell_index = 0; cell_index < 4; cell_index++) + { + // calculated for the cell in the current row + row = row_index + (cell_index * 4); + // calculated for the cell in the current column + col = cell_index + (col_index * 4); + + return_mat.e[cell] += this->e[row] * rhs.e[col]; + } + } + } + + *this = return_mat; + + return *this; + } + + /// \brief Divides the matrix by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The divided matrix. + template + mat& operator/=(const S& rhs) + { + #if defined(_MSC_VER) + #pragma warning(push) + + // The operator/= throws a warning when a mat*f is divided by a double. + #pragma warning(disable:4244) + #endif + + for (size_t i = 0; i < 4 * 4; i++) + e[i] /= rhs; + + #if defined (_MSC_VER) + #pragma warning(pop) + #endif + + return *this; + } + + /// \brief Adds a matrix to this matrix. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + template + mat& operator+=(const mat<4, 4, S>& rhs) + { + for (size_t i = 0; i < 4*4; i++) + e[i] += rhs.e[i]; + + return *this; + } + + /// \brief Subtracts a matrix from this matrix. + /// + /// \param[in] rhs The right-side matrix. + /// \return The resulting matrix. + template + mat& operator-=(const mat<4, 4, S>& rhs) + { + for (size_t i = 0; i < 4*4; i++) + e[i] -= rhs.e[i]; + + return *this; + } + + // Public Methods ///////////////////////////////////////////////////////// + +public: + + /// \brief The matrix's row dimension. + /// + /// \return The matrix's row dimension. + size_t dimRow() const { return 4; } + + /// \brief The matrix's column dimension. + /// + /// \return The matrix's column dimension. + size_t dimCol() const { return 4; } + + template + void copy(const mat<4, 4, S>& rhs) + { + *this = rhs; + } + + /// \brief Negates the matrix. + /// + /// \return The negated matrix. + mat neg() const + { + mat nm; + + for (size_t i = 0; i < 4 * 4; i++) + nm.e[i] = -e[i]; + + return nm; + } + + /// \brief Multiplies the matrix by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The multiplied matrix. + mat mult(const double& scalar) const + { + mat nm; + + for (size_t i = 0; i < 4*4; i++) + nm.e[i] = e[i] * scalar; + + return nm; + } + + //mat mult(const mat& rhs) + + /// \brief Divides the matrix by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The divided matrix. + mat div(const double& scalar) const + { + mat nm; + + for (size_t i = 0; i < 4*4; i++) + nm.e[i] = e[i] / scalar; + + return nm; + } + + /// \brief Adds a matrix to this matrix. + /// + /// \param[in] toAdd The matrix to add. + /// \return The resulting matrix. + mat add(const mat& toAdd) const + { + mat nm; + + for (size_t i = 0; i < 4*4; i++) + nm.e[i] = e[i] + toAdd.e[i]; + + return nm; + } + + /// \brief Subtracts a matrix from this matrix. + /// + /// \param[in] toSub The matrix to subtract from this. + /// \return The resulting matrix. + mat sub(const mat& toSub) const + { + mat nm; + + for (size_t i = 0; i < 4*4; i++) + nm.e[i] = e[i] - toSub.e[i]; + + return nm; + } + + /// \brief Transposes the matrix. + /// + /// \return The computed transpose. + mat<4, 4, T> transpose() const + { + mat<4, 4, T> nm; + + for (size_t row = 0; row < 4; row++) + { + for (size_t col = 0; col < 4; col++) + { + nm.e[row * 4 + col] = e[col * 4 + row]; + } + } + + return nm; + } +}; + +#if defined (_MSC_VER) + #pragma warning(pop) +#endif + +// Operator Overloads ///////////////////////////////////////////////////////// + +/// \brief Multiplies a matrix by a scalar. +/// +/// \param[in] lhs The matrix. +/// \param[in] rhs The scalar. +/// \return The result. +template +mat operator*(mat lhs, const S& rhs) +{ + lhs *= rhs; + + return lhs; +} + +/// \brief Multiplies a matrix by a scalar. +/// +/// \param[in] lhs The scalar. +/// \param[in] rhs The matrix. +/// \return The result. +template +mat operator*(const S& lhs, mat rhs) +{ + rhs *= lhs; + + return rhs; +} + +/// \brief Multiplies two matrices together. +/// +/// \param[in] lhs The left-side matrix. +/// \param[in] rhs The right-side matrix. +/// \return The result. +template +mat operator*(mat&lhs, const mat& rhs) +{ + mat tmp = lhs; + tmp *= rhs; + + return tmp; +} + +/// \brief Divides a matrix by a scalar. +/// +/// \param[in] lhs The matrix. +/// \param[in] rhs The scalar. +/// \return The result. +template +mat operator/(mat lhs, const S& rhs) +{ + lhs /= rhs; + + return lhs; +} + +/// \brief Adds two matrices together. +/// +/// \param[in] lhs The left-side matrix. +/// \param[in] rhs The right-side matrix. +/// \return The resulting matrix. +template +mat operator+(mat lhs, const mat& rhs) +{ + lhs += rhs; + + return lhs; +} + +/// \brief Subtracts a matrix from another matrix. +/// +/// \param[in] lhs The left-side matrix. +/// \param[in] rhs The right-side matrix. +/// \return The resulting matrix. +template +mat operator-(mat lhs, const mat& rhs) +{ + lhs -= rhs; + + return lhs; +} + +// Specific Typedefs ////////////////////////////////////////////////////////// + +/// \brief 2x2 matrix using float as its underlying data type. +typedef mat<2> mat2; + +/// \brief 3x3 matrix using float as its underlying data type. +typedef mat<3> mat3; + +/// \brief 4x4 matrix using float as its underlying data type. +typedef mat<4> mat4; + +/// \brief 2x2 matrix using float as its underlying data type. +typedef mat<2> mat22; + +/// \brief 3x3 matrix using float as its underlying data type. +typedef mat<3> mat33; + +/// \brief 4x4 matrix using float as its underlying data type. +typedef mat<4> mat44; + +/// \brief 2x2 matrix using float as its underlying data type. +typedef mat<2, 2, float> mat2f; + +/// \brief 3x3 matrix using float as its underlying data type. +typedef mat<3, 3, float> mat3f; + +/// \brief 4x4 matrix using float as its underlying data type. +typedef mat<4, 4, float> mat4f; + +/// \brief 2x2 matrix using double as its underlying data type. +typedef mat<2, 2, double> mat2d; + +/// \brief 3x3 matrix using double as its underlying data type. +typedef mat<3, 3, double> mat3d; + +/// \brief 4x4 matrix using double as its underlying data type. +typedef mat<4, 4, double> mat4d; + +/// \brief 2x2 matrix using long double as its underlying data type. +typedef mat<2, 2, long double> mat2ld; + +/// \brief 3x3 matrix using long double as its underlying data type. +typedef mat<3, 3, long double> mat3ld; + +/// \brief 4x4 matrix using long double as its underlying data type. +typedef mat<4, 4, long double> mat4ld; + +/// \brief 2x2 matrix using float as its underlying data type. +typedef mat<2, 2, float> mat22f; + +/// \brief 3x3 matrix using float as its underlying data type. +typedef mat<3, 3, float> mat33f; + +/// \brief 4x4 matrix using float as its underlying data type. +typedef mat<4, 3, float> mat44f; + +/// \brief 2x2 matrix using double as its underlying data type. +typedef mat<2, 2, double> mat22d; + +/// \brief 3x3 matrix using double as its underlying data type. +typedef mat<3, 3, double> mat33d; + +/// \brief 4x4 matrix using double as its underlying data type. +typedef mat<4, 4, double> mat44d; + +/// \brief 2x2 matrix using long double as its underlying data type. +typedef mat<2, 2, long double> mat22ld; + +/// \brief 3x3 matrix using long double as its underlying data type. +typedef mat<3, 3, long double> mat33ld; + +/// \brief 4x4 matrix using long double as its underlying data type. +typedef mat<4, 4, long double> mat44ld; + +// Common functions for working with matrices. + +/// \brief Provides a method to generate a representable string from a provided +/// matrix. +/// +/// \param[in] m The matrix to convert to string. +/// \return The string representation. +template std::string str(mat m) +{ + std::stringstream ss; + ss << "["; + + for (size_t row_index = 0; row_index < m.dimRow(); row_index++) + { + ss << "("; + + for (size_t col_index = 0; col_index < m.dimCol(); col_index++) + { + ss << m(row_index, col_index); + + if (col_index + 1 < m.dimCol()) + ss << "; "; + } + + ss << ")"; + } + + ss << "]"; + + return ss.str(); +} + +/// \brief Overloads the ostream << operator for easy usage in displaying +/// matrices. +/// +/// \param[in] out The ostream being output to. +/// \param[in] m The matrix to output to ostream. +/// \return Reference to the current ostream. +template std::ostream& operator<<(std::ostream& out, mat m) +{ + out << str(m); + return out; +} + +} +} + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/math/position.h b/ext/vnproglib-1.1.0.115/include/vn/math/position.h new file mode 100755 index 0000000..a387686 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/math/position.h @@ -0,0 +1,48 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#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 + }; + +private: + + PositionD(PositionType type, vec3d pos); + +public: + + /// \brief Creates a new PositionD from a latitude, longitude, altitude. + /// + /// \param[in] lla The position expressed as a latitude, longitude, altitude. + /// \return The new PositionD. + static PositionD fromLla(vec3d lla); + + /// \brief Creates a new PositionD from an earth-centered, earth-fixed. + /// + /// \param[in] ecef The position expressed as an earth-centered, earth-fixed. + /// \return The new PositionD. + static PositionD fromEcef(vec3d ecef); + +private: + PositionType _underlyingType; + vec3d _data; +}; + +} +} + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/math/vector.h b/ext/vnproglib-1.1.0.115/include/vn/math/vector.h new file mode 100755 index 0000000..bf3f0b8 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/math/vector.h @@ -0,0 +1,1377 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +/// \file +/// {COMMON_HEADER} +/// +/// \section DESCRIPTION +/// This header file provides may types for working with vectors. +#ifndef _VN_MATH_VECTOR_H_ +#define _VN_MATH_VECTOR_H_ + +#include +#include +#include +#include + +#include "vn/exceptions.h" +#include "vn/int.h" + +namespace vn { +namespace math { + +/// \brief Template for a Euclidean vector. +template +struct vec +{ + // Public Members ///////////////////////////////////////////////////////// + +public: + + /// \brief The vector's components. + T c[tdim]; + + // Constructors /////////////////////////////////////////////////////////// + +public: + + /// \brief Creates a new vector with uninitialized components. + vec() { } + + /// \brief Creates new vector with components initialized to val. + /// + /// \param[in] val The initialization value. + explicit vec(T val) + { + std::fill_n(c, tdim, val); + } + + // Helper Methods ///////////////////////////////////////////////////////// + +public: + + /// \brief Vector with all of its components set to 0. + /// + /// \return The 0 vector. + static vec zero() + { + return vec(0); + } + + /// \brief Vector with all of its components set to 1. + /// + /// \return The 1 vector. + static vec one() + { + return vec(1); + } + + // Operator Overloads ///////////////////////////////////////////////////// + +public: + + /// \brief Indexing into the vector's components. + /// + /// \param[in] index 0-based component index. + /// \exception dimension_error The index exceeded the dimension of the vector. + T& operator[](size_t index) + { + return const_cast(static_cast(*this)[index]); + } + + /// \brief Indexing into the vector's components. + /// + /// \param[in] index 0-based component index. + /// \exception dimension_error The index exceeded the dimension of the vector. + const T& operator[](size_t index) const + { + assert(index < tdim); + + return c[index]; + } + + /// \brief Negates the vector. + /// + /// \return The negated vector. + vec operator-() const + { + return neg(); + } + + /// \brief Adds a vector to this vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + vec& operator+=(const vec& rhs) + { + for (size_t i = 0; i < tdim; i++) + c[i] += rhs.c[i]; + + return *this; + } + + /// \brief Subtracts a vector from this vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + vec& operator-=(const vec& rhs) + { + for (size_t i = 0; i < tdim; i++) + c[i] -= rhs.c[i]; + + return *this; + } + + /// \brief Multiplies the vector by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The multiplied vector. + vec& operator*=(const T& rhs) + { + for (size_t i = 0; i < tdim; i++) + c[i] *= rhs; + + return *this; + } + + /// \brief Divides the vector by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The divided vector. + vec& operator/=(const T & rhs) + { + for (size_t i = 0; i < tdim; i++) + c[i] /= rhs; + + return *this; + } + + // Public Methods ///////////////////////////////////////////////////////// + +public: + + /// \brief The vector's dimension. + /// + /// \return The vector's dimension. + size_t dim() const { return tdim; } + + /// \brief Negates the vector. + /// + /// \return The negated vector. + vec neg() const + { + vec v; + + for (size_t i = 0; i < tdim; i++) + v.c[i] = -c[i]; + + return v; + } + + /// \brief The vector's magnitude. + /// + /// \return The magnitude. + T mag() const + { + T sumOfSquares = 0; + + for (size_t i = 0; i < tdim; i++) + sumOfSquares += c[i] * c[i]; + + return sqrt(sumOfSquares); + } + + /// \brief Adds a vector to this vector. + /// + /// \param[in] toAdd The vector to add. + /// \return The resulting vector. + vec add(const vec& toAdd) const + { + vec v; + + for (size_t i = 0; i < tdim; i++) + v.c[i] = c[i] + toAdd.c[i]; + + return v; + } + + /// \brief Subtracts a vector from this vector. + /// + /// \param[in] to_sub The vector to subtract from this. + /// \return The resulting vector. + vec sub(const vec& to_sub) const + { + vec v; + + for (size_t i = 0; i < tdim; i++) + v.c[i] = c[i] - to_sub.c[i]; + + return v; + } + + /// \brief Multiplies the vector by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The multiplied vector. + vec mult(const double& scalar) const + { + vec v; + + for (size_t i = 0; i < tdim; i++) + v.c[i] = c[i] * scalar; + + return v; + } + + /// \brief Divides the vector by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The divided vector. + vec div(const double& scalar) const + { + vec v; + + for (size_t i = 0; i < tdim; i++) + v.c[i] = c[i] / scalar; + + return v; + } + + /// \brief Normalizes the vector. + /// + /// \return The normalized vector. + vec norm() const + { + vec v; + + T m = mag(); + + for (size_t i = 0; i < tdim; i++) + v.c[i] = c[i] / m; + + return v; + } + + /// \brief Computes the dot product of this and the provided vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The computed dot product. + T dot(const vec& rhs) const + { + T runningSum = 0; + + for (size_t i = 0; i < tdim; i++) + runningSum += c[i] * rhs.c[i]; + + return runningSum; + } + +}; + +// Specializations //////////////////////////////////////////////////////////// + +#if defined(_MSC_VER) + #pragma warning(push) + + // Disable warning about 'nonstandard extension used : nameless struct/union'. + #pragma warning(disable:4201) +#endif + +/// \brief Vector with 2 component specialization. +template +struct vec<2, T> +{ + + // Public Members ///////////////////////////////////////////////////////// + +public: + + union + { + struct + { + /// \brief X (0-component). + T x; + + /// \brief Y (1-component). + T y; + }; + + /// \brief The vector's components. + T c[2]; + }; + + // Constructors /////////////////////////////////////////////////////////// + +public: + + /// \brief Creates a new vector with uninitialized components. + vec() { } + + /// \brief Creates new vector with components initialized to val. + /// + /// \param[in] val The initialization value. + explicit vec(T val) : x(val), y(val) { } + + /// \brief Creates a new vector with its components inintialized to the + /// provided values. + /// + /// \param[in] x_val The x value. + /// \param[in] y_val The y value. + vec(T x_val, T y_val) : x(x_val), y(y_val) { } + + // Helper Methods ///////////////////////////////////////////////////////// + +public: + + /// \brief Vector with all of its components set to 0. + /// + /// \return The 0 vector. + static vec zero() + { + return vec<2, T>(0); + } + + /// \brief Vector with all of its components set to 1. + /// + /// \return The 1 vector. + static vec one() + { + return vec<2, T>(1); + } + + /// \brief Unit vector pointing in the X (0-component) direction. + /// + /// \return The unit vector. + static vec unitX() + { + return vec<2, T>(1, 0); + } + + /// \brief Unit vector pointing in the Y (1-component) direction. + /// + /// \return The unit vector. + static vec unitY() + { + return vec<2, T>(0, 1); + } + + + // Operator Overloads ///////////////////////////////////////////////////// + +public: + + /// \brief Indexing into the vector's components. + /// + /// \param[in] index 0-based component index. + /// \exception dimension_error The index exceeded the dimension of the vector. + T& operator[](size_t index) + { + return const_cast(static_cast(*this)[index]); + } + + /// \brief Indexing into the vector's components. + /// + /// \param[in] index 0-based component index. + /// \exception dimension_error The index exceeded the dimension of the vector. + const T& operator[](size_t index) const + { + assert(index < 2); + + return c[index]; + } + + /// \brief Negates the vector. + /// + /// \return The negated vector. + vec operator-() const + { + return neg(); + } + + /// \brief Adds a vector to this vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + vec& operator+=(const vec& rhs) + { + x += rhs.x; + y += rhs.y; + + return *this; + } + + /// \brief Subtracts a vector from this vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + vec& operator-=(const vec& rhs) + { + x -= rhs.x; + y -= rhs.y; + + return *this; + } + + /// \brief Multiplies the vector by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The multiplied vector. + vec& operator*=(const T& rhs) + { + x *= rhs; + y *= rhs; + + return *this; + } + + /// \brief Divides the vector by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The divided vector. + vec& operator/=(const T & rhs) + { + x /= rhs; + y /= rhs; + + return *this; + } + + // Public Methods ///////////////////////////////////////////////////////// + +public: + + /// \brief The vector's dimension. + /// + /// \return The vector's dimension. + size_t dim() const { return 2; } + + /// \brief Negates the vector. + /// + /// \return The negated vector. + vec neg() const + { + // TODO: Issue when the underlying type is an unsigned integer. + #if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable:4146) + #endif + + return vec(-x, -y); + + #if defined (_MSC_VER) + #pragma warning(pop) + #endif + } + + /// \brief The vector's magnitude. + /// + /// \return The magnitude. + T mag() const + { + // TODO: Might want this method to return a float even if the underlying + // data type is integer. + #if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable:4244) + #endif + + #if (defined(_MSC_VER) && _MSC_VER <= 1600) + // HACK: Visual Studio 2010 has trouble determining the correct 'sqrt' + // function for the template int32_t. + return sqrt(static_cast(x*x + y*y)); + #else + // HACK: + return sqrt(static_cast(x*x + y*y)); + //return sqrt(x*x + y*y); + #endif + + #if defined (_MSC_VER) + #pragma warning(pop) + #endif + } + + /// \brief Adds a vector to this vector. + /// + /// \param[in] toAdd The vector to add. + /// \return The resulting vector. + vec add(const vec& toAdd) const + { + return vec(x + toAdd.x, y + toAdd.y); + } + + /// \brief Subtracts a vector from this vector. + /// + /// \param[in] to_sub The vector to subtract from this. + /// \return The resulting vector. + vec sub(const vec& to_sub) const + { + return vec(x - to_sub.x, y - to_sub.y); + } + + /// \brief Multiplies the vector by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The multiplied vector. + vec mult(const double& scalar) const + { + return vec(x * scalar, y * scalar); + } + + /// \brief Divides the vector by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The divided vector. + vec div(const double& scalar) const + { + return vec(x / scalar, y / scalar); + } + + /// \brief Normalizes the vector. + /// + /// \return The normalized vector. + vec norm() const + { + T m = mag(); + + return vec(x / m, y / m); + } + + /// \brief Computes the dot product of this and the provided vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The computed dot product. + T dot(const vec& rhs) const + { + return x*rhs.x + y*rhs.y; + } + +}; + + +/// \brief Vector with 3 component specialization. +template +struct vec<3, T> +{ + + // Public Members ///////////////////////////////////////////////////////// + +public: + + union + { + struct + { + /// \brief X (0-component). + T x; + + /// \brief Y (1-component). + T y; + + /// \brief Z (2-component). + T z; + }; + + struct + { + /// \brief Red (0-component). + T r; + + /// \brief Green (1-component). + T g; + + /// \brief Blue (2-component). + T b; + }; + + // Union of template class with constructor not allowed until C++11. + #if __cplusplus >= 201103L + + /// \brief XY (0,1-components). + vec<2, T> xy; + + #endif + + /// \brief The vector's components. + T c[3]; + }; + + // Constructors /////////////////////////////////////////////////////////// + +public: + + /// \brief Creates a new vector with uninitialized components. + vec() { } + + /// \brief Creates new vector with components initialized to val. + /// + /// \param[in] val The initialization value. + explicit vec(T val) : x(val), y(val), z(val) { } + + /// \brief Creates a new vector with its components initialized to the + /// provided values. + /// + /// \param[in] x_val The x value. + /// \param[in] y_val The y value. + /// \param[in] z_val The z value. + vec(const T& x_val, const T& y_val, const T& z_val) : x(x_val), y(y_val), z(z_val) { } + + // Helper Methods ///////////////////////////////////////////////////////// + +public: + + /// \brief Vector with all of its components set to 0. + /// + /// \return The 0 vector. + static vec zero() + { + return vec<3, T>(0); + } + + /// \brief Vector with all of its components set to 1. + /// + /// \return The 1 vector. + static vec one() + { + return vec<3, T>(1); + } + + /// \brief Unit vector pointing in the X (0-component) direction. + /// + /// \return The unit vector. + static vec unitX() + { + return vec<3, T>(1, 0, 0); + } + + /// \brief Unit vector pointing in the Y (1-component) direction. + /// + /// \return The unit vector. + static vec unitY() + { + return vec<3, T>(0, 1, 0); + } + + /// \brief Unit vector pointing in the Z (2-component) direction. + /// + /// \return The unit vector. + static vec unitZ() + { + return vec<3, T>(0, 0, 1); + } + + // Operator Overloads ///////////////////////////////////////////////////// + +public: + + /// \brief Indexing into the vector's components. + /// + /// \param[in] index 0-based component index. + /// \exception dimension_error The index exceeded the dimension of the vector. + T& operator[](size_t index) + { + return const_cast(static_cast(*this)[index]); + } + + /// \brief Indexing into the vector's components. + /// + /// \param[in] index 0-based component index. + /// \exception dimension_error The index exceeded the dimension of the vector. + const T& operator[](size_t index) const + { + assert(index < 3); + + return c[index]; + } + + /// \brief Negates the vector. + /// + /// \return The negated vector. + vec operator-() const + { + return neg(); + } + + /// \brief Adds a vector to this vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + vec& operator+=(const vec& rhs) + { + for (size_t i = 0; i < 3; i++) + c[i] += rhs.c[i]; + + return *this; + } + + /// \brief Subtracts a vector from this vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + vec& operator-=(const vec& rhs) + { + for (size_t i = 0; i < 3; i++) + c[i] -= rhs.c[i]; + + return *this; + } + + /// \brief Multiplies the vector by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The multiplied vector. + vec& operator*=(const T& rhs) + { + for (size_t i = 0; i < 3; i++) + c[i] *= rhs; + + return *this; + } + + /// \brief Divides the vector by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The divided vector. + vec& operator/=(const T & rhs) + { + for (size_t i = 0; i < 3; i++) + c[i] /= rhs; + + return *this; + } + + // Public Methods ///////////////////////////////////////////////////////// + +public: + + /// \brief The vector's dimension. + /// + /// \return The vector's dimension. + size_t dim() const { return 3; } + + /// \brief Negates the vector. + /// + /// \return The negated vector. + vec neg() const + { + // TODO: Issue when the underlying type is an unsigned integer. + #if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable:4146) + #endif + + return vec(-x, -y, -z); + + #if defined (_MSC_VER) + #pragma warning(pop) + #endif + } + + /// \brief The vector's magnitude. + /// + /// \return The magnitude. + T mag() const + { + // TODO: Might want this method to return a float even if the underlying + // data type is integer. + #if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable:4244) + #endif + + #if (defined(_MSC_VER) && _MSC_VER <= 1600) + // HACK: Visual Studio 2010 has trouble determining the correct 'sqrt' + // function for the template int32_t. + return sqrt(static_cast(x*x + y*y + z*z)); + #else + // HACK: + return sqrt(static_cast(x*x + y*y + z*z)); + //return sqrt(x*x + y*y + z*z); + #endif + + #if defined (_MSC_VER) + #pragma warning(pop) + #endif + } + + /// \brief Adds a vector to this vector. + /// + /// \param[in] toAdd The vector to add. + /// \return The resulting vector. + vec add(const vec& toAdd) const + { + return vec(x + toAdd.x, y + toAdd.y, z + toAdd.z); + } + + /// \brief Subtracts a vector from this vector. + /// + /// \param[in] to_sub The vector to subtract from this. + /// \return The resulting vector. + vec sub(const vec& to_sub) const + { + return vec(x - to_sub.x, y - to_sub.y, z - to_sub.z); + } + + /// \brief Multiplies the vector by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The multiplied vector. + vec mult(const double& scalar) const + { + return vec(x * scalar, y * scalar, z * scalar); + } + + /// \brief Divides the vector by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The divided vector. + vec div(const double& scalar) const + { + return vec(x / scalar, y / scalar, z / scalar); + } + + /// \brief Normalizes the vector. + /// + /// \return The normalized vector. + vec norm() const + { + T m = mag(); + + return vec(x / m, y / m, z / m); + } + + /// \brief Computes the dot product of this and the provided vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The computed dot product. + T dot(const vec& rhs) const + { + return x*rhs.x + y*rhs.y + z*rhs.z; + } + + /// \brief Computes the cross product of this and the provided vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The computed cross product. + /// \exception dimension_error The dimension of the vector is not 3. + vec<3, T> cross(const vec<3, T>& rhs) const + { + vec<3, T> v; + + v.c[0] = c[1] * rhs.c[2] - c[2] * rhs.c[1]; + v.c[1] = c[2] * rhs.c[0] - c[0] * rhs.c[2]; + v.c[2] = c[0] * rhs.c[1] - c[1] * rhs.c[0]; + + return v; + } + +}; + +/// \brief Vector with 4 component specialization. +template +struct vec<4, T> +{ + + // Public Members ///////////////////////////////////////////////////////// + +public: + + union + { + struct + { + /// \brief X (0-component). + T x; + + /// \brief Y (1-component). + T y; + + /// \brief Z (2-component). + T z; + + /// \brief W (3-component). + T w; + }; + + struct + { + /// \brief Red (0-component). + T r; + + /// \brief Green (1-component). + T g; + + /// \brief Blue (2-component). + T b; + + /// \brief Alpha (3-component). + T a; + }; + + // Union of template class with constructor not allowed until C++11. + #if __cplusplus >= 201103L + + /// \brief XY (0,1-components). + vec<2, T> xy; + + /// \brief XYZ (0,1,2-components). + vec<3, T> xyz; + + /// \brief RGB (0,1,2-components). + vec<3, T> rgb; + + #endif + + /// \brief The vector's components. + T c[4]; + }; + + // Constructors /////////////////////////////////////////////////////////// + +public: + + /// \brief Creates a new vector with uninitialized components. + vec() { } + + /// \brief Creates new vector with components initialized to val. + /// + /// \param[in] val The initialization value. + explicit vec(T val) : x(val), y(val), z(val), w(val) { } + + /// \brief Creates a new vector with its components inintialized to the + /// provided values. + /// + /// \param[in] x_val The x value. + /// \param[in] y_val The y value. + /// \param[in] z_val The z value. + /// \param[in] w_val The w value. + vec(T x_val, T y_val, T z_val, T w_val) : x(x_val), y(y_val), z(z_val), w(w_val) { } + + // Helper Methods ///////////////////////////////////////////////////////// + +public: + + /// \brief Vector with all of its components set to 0. + /// + /// \return The 0 vector. + static vec zero() + { + return vec<4, T>(0); + } + + /// \brief Vector with all of its components set to 1. + /// + /// \return The 1 vector. + static vec one() + { + return vec<4, T>(1); + } + + /// \brief Unit vector pointing in the X (0-component) direction. + /// + /// \return The unit vector. + static vec unitX() + { + return vec<4, T>(1, 0, 0, 0); + } + + /// \brief Unit vector pointing in the Y (1-component) direction. + /// + /// \return The unit vector. + static vec unitY() + { + return vec<4, T>(0, 1, 0, 0); + } + + /// \brief Unit vector pointing in the Z (2-component) direction. + /// + /// \return The unit vector. + static vec unitZ() + { + return vec<4, T>(0, 0, 1, 0); + } + + /// \brief Unit vector pointing in the W (3-component) direction. + /// + /// \return The unit vector. + static vec unitW() + { + return vec<4, T>(0, 0, 0, 1); + } + // Operator Overloads ///////////////////////////////////////////////////// + +public: + + /// \brief Indexing into the vector's components. + /// + /// \param[in] index 0-based component index. + /// \exception dimension_error The index exceeded the dimension of the vector. + T& operator[](size_t index) + { + return const_cast(static_cast(*this)[index]); + } + + /// \brief Indexing into the vector's components. + /// + /// \param[in] index 0-based component index. + /// \exception dimension_error The index exceeded the dimension of the vector. + const T& operator[](size_t index) const + { + assert(index < 4); + + return c[index]; + } + + /// \brief Negates the vector. + /// + /// \return The negated vector. + vec operator-() const + { + return neg(); + } + + /// \brief Adds a vector to this vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + vec& operator+=(const vec& rhs) + { + x += rhs.x; + y += rhs.y; + z += rhs.z; + w += rhs.w; + + return *this; + } + + /// \brief Subtracts a vector from this vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The resulting vector. + vec& operator-=(const vec& rhs) + { + x -= rhs.x; + y -= rhs.y; + z -= rhs.z; + w -= rhs.w; + + return *this; + } + + /// \brief Multiplies the vector by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The multiplied vector. + vec& operator*=(const T& rhs) + { + x *= rhs; + y *= rhs; + z *= rhs; + w *= rhs; + + return *this; + } + + /// \brief Divides the vector by a scalar. + /// + /// \param[in] rhs The scalar. + /// \return The divided vector. + vec& operator/=(const T & rhs) + { + x /= rhs; + y /= rhs; + z /= rhs; + w /= rhs; + + return *this; + } + + // Public Methods ///////////////////////////////////////////////////////// + +public: + + /// \brief The vector's dimension. + /// + /// \return The vector's dimension. + size_t dim() const { return 4; } + + /// \brief Negates the vector. + /// + /// \return The negated vector. + vec neg() const + { + // TODO: Issue when the underlying type is an unsigned integer. + #if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable:4146) + #endif + + return vec(-x, -y, -z, -w); + + #if defined (_MSC_VER) + #pragma warning(pop) + #endif + } + + /// \brief The vector's magnitude. + /// + /// \return The magnitude. + T mag() const + { + // TODO: Might want this method to return a float even if the underlying + // data type is integer. + #if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable:4244) + #endif + + #if (defined(_MSC_VER) && _MSC_VER <= 1600) + // HACK: Visual Studio 2010 has trouble determining the correct 'sqrt' + // function for the template int32_t. + return sqrt(static_cast(x*x + y*y + z*z + w*w)); + #else + // HACK: + return sqrt(static_cast(x*x + y*y + z*z + w*w)); + //return sqrt(x*x + y*y + z*z + w*w); + #endif + + #if defined (_MSC_VER) + #pragma warning(pop) + #endif + } + + /// \brief Adds a vector to this vector. + /// + /// \param[in] toAdd The vector to add. + /// \return The resulting vector. + vec add(const vec& toAdd) const + { + return vec(x + toAdd.x, y + toAdd.y, z + toAdd.z, w + toAdd.w); + } + + /// \brief Subtracts a vector from this vector. + /// + /// \param[in] to_sub The vector to subtract from this. + /// \return The resulting vector. + vec sub(const vec& to_sub) const + { + return vec(x - to_sub.x, y - to_sub.y, z - to_sub.z, w - to_sub.w); + } + + /// \brief Multiplies the vector by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The multiplied vector. + vec mult(const double& scalar) const + { + return vec(x * scalar, y * scalar, z * scalar, w * scalar); + } + + /// \brief Divides the vector by a scalar. + /// + /// \param[in] scalar The scalar value. + /// \return The divided vector. + vec div(const double& scalar) const + { + return vec(x / scalar, y / scalar, z / scalar, w / scalar); + } + + /// \brief Normalizes the vector. + /// + /// \return The normalized vector. + vec norm() const + { + T m = mag(); + + return vec(x / m, y / m, z / m, w / m); + } + + /// \brief Computes the dot product of this and the provided vector. + /// + /// \param[in] rhs The right-side vector. + /// \return The computed dot product. + T dot(const vec& rhs) const + { + return x*rhs.x + y*rhs.y + z*rhs.z + w*rhs.w; + } +}; + +#if defined (_MSC_VER) + #pragma warning(pop) +#endif + +// Operator Overloads ///////////////////////////////////////////////////////// + +/// \brief Adds two vectors together. +/// +/// \param[in] lhs The left-side vector. +/// \param[in] rhs The right-side vector. +/// \return The resulting vector. +template +vec operator+(vec lhs, const vec& rhs) +{ + lhs += rhs; + + return lhs; +} + +/// \brief Subtracts a vector from another vector. +/// +/// \param[in] lhs The left-side vector. +/// \param[in] rhs The right-side vector. +/// \return The resulting vector. +template +vec operator-(vec lhs, const vec& rhs) +{ + lhs -= rhs; + + return lhs; +} + +#if defined(_MSC_VER) + #pragma warning(push) + + // The operator* and operator/ throw a warning when a vec*f is multiplied by a double. + #pragma warning(disable:4244) +#endif + +/// \brief Multiplies a vector by a scalar. Done both ways for python +/// +/// \param[in] lhs The scalar. +/// \param[in] rhs The vector. +/// \return The result. +template +vec operator*(vec lhs, const S& rhs) +{ + lhs *= rhs; + + return lhs; +} + +/// \brief Multiplies a vector by a scalar. Done both ways for Python +/// +/// \param[in] lhs The vector. +/// \param[in] rhs The scalar. +/// \return The result. +template +vec operator*(const S& rhs, vec lhs) +{ + lhs *= rhs; + + return lhs; +} + +/// \brief Divides a vector by a scalar. +/// +/// \param[in] lhs The vector. +/// \param[in] rhs The scalar. +/// \return The result. +template +vec operator/(vec lhs, const S& rhs) +{ + lhs /= rhs; + + return lhs; +} + +#if defined (_MSC_VER) + #pragma warning(pop) +#endif + +// Specific Typedefs ////////////////////////////////////////////////////////// + +/// \brief 2-component vector using float as its underlying data type. +typedef vec<2> vec2; + +/// \brief 3-component vector using float as its underlying data type. +typedef vec<3> vec3; + +/// \brief 4-component vector using float as its underlying data type. +typedef vec<4> vec4; + +/// \brief 2-component vector using float as its underlying data type. +typedef vec<2, float> vec2f; + +/// \brief 3-component vector using float as its underlying data type. +typedef vec<3, float> vec3f; + +/// \brief 4-component vector using float as its underlying data type. +typedef vec<4, float> vec4f; + +/// \brief 2-component vector using double as its underlying data type. +typedef vec<2, double> vec2d; + +/// \brief 3-component vector using double as its underlying data type. +typedef vec<3, double> vec3d; + +/// \brief 4-component vector using double as its underlying data type. +typedef vec<4, double> vec4d; + +/// \brief 2-component vector using long double as its underlying data type. +typedef vec<2, long double> vec2ld; + +/// \brief 3-component vector using long double as its underlying data type. +typedef vec<3, long double> vec3ld; + +/// \brief 4-component vector using long double as its underlying data type. +typedef vec<4, long double> vec4ld; + +/// \brief 2-component vector using int32_t as its underlying data type. +typedef vec<2, int32_t> vec2i32; + +/// \brief Namenclature used by OpenGL API. +typedef vec2i32 ivec2; + +/// \brief 3-component vector using int32_t as its underlying data type. +typedef vec<3, int32_t> vec3i32; + +/// \brief 4-component vector using int32_t as its underlying data type. +typedef vec<4, int32_t> vec4i32; + +/// \brief 2-component vector using uint32_t as its underlying data type. +typedef vec<2, uint32_t> vec2u32; + +/// \brief 3-component vector using uint32_t as its underlying data type. +typedef vec<3, uint32_t> vec3u32; + +/// \brief 4-component vector using uint32_t as its underlying data type. +typedef vec<4, uint32_t> vec4u32; + +// Common functions for working with vectors. + +/// \brief Provides a method to generate a representable string from a provided +/// vector. +/// +/// \param[in] v The vector to convert to string. +/// \return The string representation. +template std::string str(vec v) +{ + std::stringstream ss; + ss << "("; + for (size_t i = 0; i < v.dim(); i++) + { + ss << v[i]; + + if (i + 1 < v.dim()) + ss << "; "; + } + ss << ")"; + + return ss.str(); +} + +/// \brief Overloads the ostream << operator for easy usage in displaying +/// vectors. +/// +/// \param[in] out The ostream being output to. +/// \param[in] v The vector to output to ostream. +/// \return Reference to the current ostream. +template std::ostream& operator<<(std::ostream& out, vec v) +{ + out << str(v); + return out; +} + +} +} + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/math/ypr.h b/ext/vnproglib-1.1.0.115/include/vn/math/ypr.h new file mode 100755 index 0000000..461d21f --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/math/ypr.h @@ -0,0 +1,89 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifndef _VN_MATH_YPR_H_ +#define _VN_MATH_YPR_H_ + +#include "vn/math/conversions.h" +#include "vn/math/vector.h" + +namespace vn { +namespace math { + +template +struct ypr +{ + // Public Members ///////////////////////////////////////////////////////// +public: + + union + { + T yaw; + T y; + }; + + union + { + T pitch; + T p; + }; + + union + { + T roll; + T r; + }; + + // Helper Methods ///////////////////////////////////////////////////////// +public: + + /// \brief Yaw, pitch, roll representing no rotation. + /// + /// \return No rotation. + static ypr noRotation() + { + return ypr(0, 0, 0); + } + + static ypr fromDegs(T yawInDegs, T pitchInDegs, T rollInDegs) + { + return ypr(deg2rad(yawInDegs), deg2rad(pitchInDegs), deg2rad(rollInDegs)); + } + + static ypr fromDegs(vec3 valuesInDegs) + { + return fromDegs(valuesInDegs.x, valuesInDegs.y, valuesInDegs.z); + } + + // Constructors /////////////////////////////////////////////////////////// + +public: + + ypr(T yawInRads, T pitchInRads, T rollInRads) : + yaw(yawInRads), + pitch(pitchInRads), + roll(rollInRads) + { } + + explicit ypr(vec3 valuesInRads) : + yaw(valuesInRads.x), + pitch(valuesInRads.y), + roll(valuesInRads.z) + { } + + // Methods //////////////////////////////////////////////////////////////// + +public: + + vec3 toVec3() + { + return vec3(yaw, pitch, roll); + } + +}; + +typedef ypr yprf; + +} +} + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/protocol/uart/packet.h b/ext/vnproglib-1.1.0.115/include/vn/protocol/uart/packet.h new file mode 100755 index 0000000..f284097 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/protocol/uart/packet.h @@ -0,0 +1,2108 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifndef _VNPROTOCOL_UART_PACKET_H_ +#define _VNPROTOCOL_UART_PACKET_H_ + +#include "vn/int.h" +#include "vn/math/vector.h" +#include "vn/math/matrix.h" +#include "vn/util/nocopy.h" +#include "vn/protocol/uart/types.h" + +namespace vn { +namespace protocol { +namespace uart { + +/// \brief Structure representing a UART packet received from the VectorNav +/// sensor. +struct vn_proglib_DLLEXPORT Packet +{ + /// \brief Array containing sizes for the binary group fields. + static const unsigned char BinaryGroupLengths[sizeof(uint8_t)*8][sizeof(uint16_t)*8]; + + /// \brief The different types of UART packets. + enum Type + { + TYPE_UNKNOWN, ///< Type is unknown. + TYPE_BINARY, ///< Binary packet. + TYPE_ASCII ///< ASCII packet. + }; + + Packet(); + + /// \brief Creates a new packet based on the provided packet data buffer. A full + /// packet is expected which contains the deliminators (i.e. "$VNRRG,1*XX\r\n"). + /// + /// \param[in] packet Pointer to buffer containing the packet. + /// \param[in] length The number of bytes in the packet. + Packet(char const* packet, size_t length); + + explicit Packet(std::string packet); + + /// \brief Copy constructor. + /// + /// \param[in] toCopy The Packet to copy. + Packet(const Packet &toCopy); + + ~Packet(); + + /// \brief Assignment operator. + /// + /// \param[in] from The packet to assign from. + /// \return Reference to the newly copied packet. + Packet& operator=(const Packet &from); + + /// \brief Returns the encapsulated data as a string. + /// + /// \return The packet data. + std::string datastr(); + + /// \brief Returns the type of packet. + /// + /// \return The type of packet. + Type type(); + + /// \brief Performs data integrity check on the data packet. + /// + /// This will perform an 8-bit XOR checksum, a CRC16-CCITT CRC, or no + /// checking depending on the provided data integrity in the packet. + /// + /// \return true if the packet passed the data integrity checks; + /// otherwise false. + bool isValid(); + + /// \brief Indicates if the packet is an ASCII error message. + /// + /// \return true if the packet is an error message; otherwise + /// false. + bool isError(); + + /// \brief Indicates if the packet is a response to a message sent to the + /// sensor. + /// + /// \return true if the packet is a response message; otherwise + /// false. + bool isResponse(); + + /// \brief Indicates if the packet is an ASCII asynchronous message. + /// + /// \return true if the packet is an ASCII asynchronous message; + /// otherwise false. + bool isAsciiAsync(); + + /// \brief Determines the type of ASCII asynchronous message this packet + /// is. + /// + /// \return The asynchronous data type of the packet. + AsciiAsync determineAsciiAsyncType(); + + /// \brief Determines if the packet is a compatible match for an expected + /// binary output message type. + /// + /// \param[in] commonGroup The Common Group configuration. + /// \param[in] timeGroup The Time Group configuration. + /// \param[in] imuGroup The IMU Group configuration. + /// \param[in] gpsGroup The GPS Group configuration. + /// \param[in] attitudeGroup The Attitude Group configuration. + /// \param[in] insGroup The INS Group configuration. + /// \return true if the packet matches the expected group + /// configuration; otherwise false. + bool isCompatible(CommonGroup commonGroup, TimeGroup timeGroup, ImuGroup imuGroup, GpsGroup gpsGroup, AttitudeGroup attitudeGroup, InsGroup insGroup); + + /// \brief Computes the expected number of bytes for a possible binary + /// packet. + /// + /// This method requires that the group fields present and the complete + /// collection of individual group description fields are present. + /// + /// \param[in] startOfPossibleBinaryPacket The start of the possible binary + /// packet (i.e. the 0xFA character). + /// + /// \return The number of bytes expected for this binary packet. + static size_t computeBinaryPacketLength(const char *startOfPossibleBinaryPacket); + + /// \brief Computes the number of bytes expected for a binary group field. + /// + /// \param[in] group The group to calculate the total for. + /// \param[in] groupField The flags for data types present. + /// \return The number of bytes for this group. + static size_t computeNumOfBytesForBinaryGroupPayload(BinaryGroup group, uint16_t groupField); + + /// \brief Parses an error packet to get the error type. + /// + /// \return The sensor error. + SensorError parseError(); + + /// \brief If the packet is a binary message, this will return the groups field. + /// \return The present groups field. + uint8_t groups(); + + /// \brief This will return the requested group field of a binary packet at the + /// specified index. + /// + /// \param[in] index The 0-based index of the requested group field. + /// \return The group field. + uint16_t groupField(size_t index); + + /// \defgroup uartPacketBinaryExtractors UART Binary Data Extractors + /// \brief This group of methods are useful for extracting data from binary + /// data packets. + /// + /// \{ + + /// \brief Extracts a uint8_t data type from a binary packet and advances + /// the next extraction point appropriately. + /// + /// \return The extracted value. + uint8_t extractUint8(); + + /// \brief Extracts a int8_t data type from a binary packet and advances + /// the next extraction point appropriately. + /// + /// \return The extracted value. + int8_t extractInt8(); + + /// \brief Extracts a uint16_t data type from a binary packet and advances + /// the next extraction point appropriately. + /// + /// \return The extracted value. + uint16_t extractUint16(); + + /// \brief Extracts a uint32_t data type from a binary packet and advances + /// the next extraction point appropriately. + /// + /// \return The extracted value. + uint32_t extractUint32(); + + /// \brief Extracts a uint64_t data type from a binary packet and advances + /// the next extraction point appropriately. + /// + /// \return The extracted value. + uint64_t extractUint64(); + + /// \brief Extracts a float fdata type from a binary packet and advances + /// the next extraction point appropriately. + /// + /// \return The extracted value. + float extractFloat(); + + /// \brief Extracts a vec3f data type from a binary packet and advances + /// the next extraction point appropriately. + /// + /// \return The extracted value. + math::vec3f extractVec3f(); + + /// \brief Extracts a vec3d data type from a binary packet and advances + /// the next extraction point appropriately. + /// + /// \return The extracted value. + math::vec3d extractVec3d(); + + /// \brief Extracts a vec4f data type from a binary packet and advances + /// the next extraction point appropriately. + /// + /// \return The extracted value. + math::vec4f extractVec4f(); + + /// \brief Extract a mat3f data type from a binary packet and advances + /// the next extraction point appropriately. + /// + /// \return The extracted value. + math::mat3f extractMat3f(); + + /// \} + + /// \brief Appends astrick (*), checksum, and newlines to command. + /// + /// \param[in] errorDetectionMode The error detection type to append to the + /// command. + /// \param[in] packet The start of the packet. Should point to the '$' + /// character. + /// \param[in] length The current running length of the packet. + /// \return The final size of the command after appending the endings. + static size_t finalizeCommand(ErrorDetectionMode errorDetectionMode, char *packet, size_t length); + + /// \defgroup regReadWriteMethods Register Read/Write Generator Methods + /// \brief This group of methods will create commands that can be used to + /// read/write the register of a VectorNav sensor. + /// + /// \{ + + /// \brief Generates a command to read the Binary Output 1 register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadBinaryOutput1(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the Binary Output 2 register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadBinaryOutput2(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the Binary Output 13 register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadBinaryOutput3(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + + /// \brief Generates a command to write to the Binary Output 1 register on a VectorNav sensor. + /// + /// The field outputGroup available on the sensor's register is not + /// necessary as this method will compute the necessary value from the + /// provided data fields. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] asyncMode The register's asyncMode field. + /// \param[in] rateDivisor The register's rateDivisor field. + /// \param[in] commonField The register's Group 1 (Common) field. + /// \param[in] timeField The register's Group 2 (Time) field. + /// \param[in] imuField The register's Group 3 (IMU) field. + /// \param[in] gpsField The register's Group 4 (GPS) field. + /// \param[in] attitudeField The register's Group 5 (Attitude) field. + /// \param[in] insField The register's Group 6 (INS) field. + /// \return The total number bytes in the generated command. + static size_t genWriteBinaryOutput1(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint16_t asyncMode, uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, uint16_t imuField, uint16_t gpsField, uint16_t attitudeField, uint16_t insField); + + /// \brief Generates a command to write to the Binary Output 2 register on a VectorNav sensor. + /// + /// The field outputGroup available on the sensor's register is not + /// necessary as this method will compute the necessary value from the + /// provided data fields. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] asyncMode The register's asyncMode field. + /// \param[in] rateDivisor The register's rateDivisor field. + /// \param[in] commonField The register's Group 1 (Common) field. + /// \param[in] timeField The register's Group 2 (Time) field. + /// \param[in] imuField The register's Group 3 (IMU) field. + /// \param[in] gpsField The register's Group 4 (GPS) field. + /// \param[in] attitudeField The register's Group 5 (Attitude) field. + /// \param[in] insField The register's Group 6 (INS) field. + /// \return The total number bytes in the generated command. + static size_t genWriteBinaryOutput2(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint16_t asyncMode, uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, uint16_t imuField, uint16_t gpsField, uint16_t attitudeField, uint16_t insField); + + /// \brief Generates a command to write to the Binary Output 3 register on a VectorNav sensor. + /// + /// The field outputGroup available on the sensor's register is not + /// necessary as this method will compute the necessary value from the + /// provided data fields. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] asyncMode The register's asyncMode field. + /// \param[in] rateDivisor The register's rateDivisor field. + /// \param[in] commonField The register's Group 1 (Common) field. + /// \param[in] timeField The register's Group 2 (Time) field. + /// \param[in] imuField The register's Group 3 (IMU) field. + /// \param[in] gpsField The register's Group 4 (GPS) field. + /// \param[in] attitudeField The register's Group 5 (Attitude) field. + /// \param[in] insField The register's Group 6 (INS) field. + /// \return The total number bytes in the generated command. + static size_t genWriteBinaryOutput3(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint16_t asyncMode, uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, uint16_t imuField, uint16_t gpsField, uint16_t attitudeField, uint16_t insField); + + + /// \brief Generates a command to write sensor settings to non-volatile memory. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genWriteSettings(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size); + + /// \brief Generates a command to tare the sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genTare(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size); + + /// \brief Generates a command to alert the sensor of a known magnetic disturbance. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] indicates if a known magnetic disturbance is present or not. + /// \return The total number bytes in the generated command. + static size_t genKnownMagneticDisturbance(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, bool isMagneticDisturbancePresent); + + /// \brief Generates a command to alert the sensor of a known acceleration disturbance. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] indicates if a known acceleration disturbance is present or not. + /// \return The total number bytes in the generated command. + static size_t genKnownAccelerationDisturbance(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, bool isAccelerationDisturbancePresent); + + /// \brief Generates a command to set the gyro bias. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genSetGyroBias(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size); + + /// \brief Generates a command to retore factory settings. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genRestoreFactorySettings(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size); + + /// \brief Generates a command to reset the sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReset(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size); + + /// \brief Generates a command to read the Serial Baud Rate register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] port The port to read from. + /// \return The total number bytes in the generated command. + static size_t genReadSerialBaudRate(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t port); + + /// \brief Generates a command to write to the Serial Baud Rate register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] baudrate The register's Baud Rate field. + /// \param[in] port The port to write to. + /// \return The total number bytes in the generated command. + static size_t genWriteSerialBaudRate(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint32_t baudrate, uint8_t port); + + /// \brief Generates a command to read the Async Data Output Type register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] port The port to read from. + /// \return The total number bytes in the generated command. + static size_t genReadAsyncDataOutputType(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t port); + + /// \brief Generates a command to write to the Async Data Output Type register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] ador The register's ADOR field. + /// \param[in] port The port to write to. + /// \return The total number bytes in the generated command. + static size_t genWriteAsyncDataOutputType(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint32_t ador, uint8_t port); + + /// \brief Generates a command to read the Async Data Output Frequency register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] port The port to read from. + /// \return The total number bytes in the generated command. + static size_t genReadAsyncDataOutputFrequency(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t port); + + /// \brief Generates a command to write to the Async Data Output Frequency register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] adof The register's ADOF field. + /// \param[in] port The port to write to. + /// \return The total number bytes in the generated command. + static size_t genWriteAsyncDataOutputFrequency(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint32_t adof, uint8_t port); + + /// \brief Generates a command to read the User Tag register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadUserTag(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the User Tag register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] tag The register's Tag field. + /// \return The total number bytes in the generated command. + static size_t genWriteUserTag(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, std::string tag); + + /// \brief Generates a command to read the Model Number register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadModelNumber(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the Hardware Revision register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadHardwareRevision(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the Serial Number register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadSerialNumber(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the Firmware Version register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadFirmwareVersion(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the Serial Baud Rate register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadSerialBaudRate(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Serial Baud Rate register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] baudrate The register's Baud Rate field. + /// \return The total number bytes in the generated command. + static size_t genWriteSerialBaudRate(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint32_t baudrate); + + /// \brief Generates a command to read the Async Data Output Type register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadAsyncDataOutputType(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Async Data Output Type register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] ador The register's ADOR field. + /// \return The total number bytes in the generated command. + static size_t genWriteAsyncDataOutputType(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint32_t ador); + + /// \brief Generates a command to read the Async Data Output Frequency register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadAsyncDataOutputFrequency(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Async Data Output Frequency register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] adof The register's ADOF field. + /// \return The total number bytes in the generated command. + static size_t genWriteAsyncDataOutputFrequency(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint32_t adof); + + /// \brief Generates a command to read the Yaw Pitch Roll register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadYawPitchRoll(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the Attitude Quaternion register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadAttitudeQuaternion(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the Quaternion, Magnetic, Acceleration and Angular Rates register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadQuaternionMagneticAccelerationAndAngularRates(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the Magnetic Measurements register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadMagneticMeasurements(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the Acceleration Measurements register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadAccelerationMeasurements(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the Angular Rate Measurements register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadAngularRateMeasurements(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the Magnetic, Acceleration and Angular Rates register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadMagneticAccelerationAndAngularRates(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the Magnetic and Gravity Reference Vectors register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadMagneticAndGravityReferenceVectors(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Magnetic and Gravity Reference Vectors register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] magRef The register's MagRef field. + /// \param[in] accRef The register's AccRef field. + /// \return The total number bytes in the generated command. + static size_t genWriteMagneticAndGravityReferenceVectors(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, math::vec3f magRef, math::vec3f accRef); + + /// \brief Generates a command to read the Filter Measurements Variance Parameters register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadFilterMeasurementsVarianceParameters(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Filter Measurements Variance Parameters register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] angularWalkVariance The register's Angular Walk Variance field. + /// \param[in] angularRateVariance The register's Angular Rate Variance field. + /// \param[in] magneticVariance The register's Magnetic Variance field. + /// \param[in] accelerationVariance The register's Acceleration Variance field. + /// \return The total number bytes in the generated command. + static size_t genWriteFilterMeasurementsVarianceParameters(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, float angularWalkVariance, math::vec3f angularRateVariance, math::vec3f magneticVariance, math::vec3f accelerationVariance); + + /// \brief Generates a command to read the Magnetometer Compensation register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadMagnetometerCompensation(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Magnetometer Compensation register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] c The register's C field. + /// \param[in] b The register's B field. + /// \return The total number bytes in the generated command. + static size_t genWriteMagnetometerCompensation(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, math::mat3f c, math::vec3f b); + + /// \brief Generates a command to read the Filter Active Tuning Parameters register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadFilterActiveTuningParameters(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Filter Active Tuning Parameters register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] magneticDisturbanceGain The register's Magnetic Disturbance Gain field. + /// \param[in] accelerationDisturbanceGain The register's Acceleration Disturbance Gain field. + /// \param[in] magneticDisturbanceMemory The register's Magnetic Disturbance Memory field. + /// \param[in] accelerationDisturbanceMemory The register's Acceleration Disturbance Memory field. + /// \return The total number bytes in the generated command. + static size_t genWriteFilterActiveTuningParameters(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, float magneticDisturbanceGain, float accelerationDisturbanceGain, float magneticDisturbanceMemory, float accelerationDisturbanceMemory); + + /// \brief Generates a command to read the Acceleration Compensation register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadAccelerationCompensation(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Acceleration Compensation register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] c The register's C field. + /// \param[in] b The register's B field. + /// \return The total number bytes in the generated command. + static size_t genWriteAccelerationCompensation(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, math::mat3f c, math::vec3f b); + + /// \brief Generates a command to read the Reference Frame Rotation register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadReferenceFrameRotation(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Reference Frame Rotation register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] c The register's C field. + /// \return The total number bytes in the generated command. + static size_t genWriteReferenceFrameRotation(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, math::mat3f c); + + /// \brief Generates a command to read the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadYawPitchRollMagneticAccelerationAndAngularRates(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the Communication Protocol Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadCommunicationProtocolControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Communication Protocol Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] serialCount The register's SerialCount field. + /// \param[in] serialStatus The register's SerialStatus field. + /// \param[in] spiCount The register's SPICount field. + /// \param[in] spiStatus The register's SPIStatus field. + /// \param[in] serialChecksum The register's SerialChecksum field. + /// \param[in] spiChecksum The register's SPIChecksum field. + /// \param[in] errorMode The register's ErrorMode field. + /// \return The total number bytes in the generated command. + static size_t genWriteCommunicationProtocolControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t serialCount, uint8_t serialStatus, uint8_t spiCount, uint8_t spiStatus, uint8_t serialChecksum, uint8_t spiChecksum, uint8_t errorMode); + + /// \brief Generates a command to read the Synchronization Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadSynchronizationControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Synchronization Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] syncInMode The register's SyncInMode field. + /// \param[in] syncInEdge The register's SyncInEdge field. + /// \param[in] syncInSkipFactor The register's SyncInSkipFactor field. + /// \param[in] syncOutMode The register's SyncOutMode field. + /// \param[in] syncOutPolarity The register's SyncOutPolarity field. + /// \param[in] syncOutSkipFactor The register's SyncOutSkipFactor field. + /// \param[in] syncOutPulseWidth The register's SyncOutPulseWidth field. + /// \return The total number bytes in the generated command. + static size_t genWriteSynchronizationControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t syncInMode, uint8_t syncInEdge, uint16_t syncInSkipFactor, uint8_t syncOutMode, uint8_t syncOutPolarity, uint16_t syncOutSkipFactor, uint32_t syncOutPulseWidth); + + /// \brief Generates a command to read the Synchronization Status register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadSynchronizationStatus(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Synchronization Status register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] syncInCount The register's SyncInCount field. + /// \param[in] syncInTime The register's SyncInTime field. + /// \param[in] syncOutCount The register's SyncOutCount field. + /// \return The total number bytes in the generated command. + static size_t genWriteSynchronizationStatus(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint32_t syncInCount, uint32_t syncInTime, uint32_t syncOutCount); + + /// \brief Generates a command to read the Filter Basic Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadFilterBasicControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Filter Basic Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] magMode The register's MagMode field. + /// \param[in] extMagMode The register's ExtMagMode field. + /// \param[in] extAccMode The register's ExtAccMode field. + /// \param[in] extGyroMode The register's ExtGyroMode field. + /// \param[in] gyroLimit The register's GyroLimit field. + /// \return The total number bytes in the generated command. + static size_t genWriteFilterBasicControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t magMode, uint8_t extMagMode, uint8_t extAccMode, uint8_t extGyroMode, math::vec3f gyroLimit); + + /// \brief Generates a command to read the VPE Basic Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadVpeBasicControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the VPE Basic Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] enable The register's Enable field. + /// \param[in] headingMode The register's HeadingMode field. + /// \param[in] filteringMode The register's FilteringMode field. + /// \param[in] tuningMode The register's TuningMode field. + /// \return The total number bytes in the generated command. + static size_t genWriteVpeBasicControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t enable, uint8_t headingMode, uint8_t filteringMode, uint8_t tuningMode); + + /// \brief Generates a command to read the VPE Magnetometer Basic Tuning register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadVpeMagnetometerBasicTuning(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the VPE Magnetometer Basic Tuning register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] baseTuning The register's BaseTuning field. + /// \param[in] adaptiveTuning The register's AdaptiveTuning field. + /// \param[in] adaptiveFiltering The register's AdaptiveFiltering field. + /// \return The total number bytes in the generated command. + static size_t genWriteVpeMagnetometerBasicTuning(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, math::vec3f baseTuning, math::vec3f adaptiveTuning, math::vec3f adaptiveFiltering); + + /// \brief Generates a command to read the VPE Magnetometer Advanced Tuning register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadVpeMagnetometerAdvancedTuning(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the VPE Magnetometer Advanced Tuning register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] minFiltering The register's MinFiltering field. + /// \param[in] maxFiltering The register's MaxFiltering field. + /// \param[in] maxAdaptRate The register's MaxAdaptRate field. + /// \param[in] disturbanceWindow The register's DisturbanceWindow field. + /// \param[in] maxTuning The register's MaxTuning field. + /// \return The total number bytes in the generated command. + static size_t genWriteVpeMagnetometerAdvancedTuning(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, math::vec3f minFiltering, math::vec3f maxFiltering, float maxAdaptRate, float disturbanceWindow, float maxTuning); + + /// \brief Generates a command to read the VPE Accelerometer Basic Tuning register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadVpeAccelerometerBasicTuning(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the VPE Accelerometer Basic Tuning register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] baseTuning The register's BaseTuning field. + /// \param[in] adaptiveTuning The register's AdaptiveTuning field. + /// \param[in] adaptiveFiltering The register's AdaptiveFiltering field. + /// \return The total number bytes in the generated command. + static size_t genWriteVpeAccelerometerBasicTuning(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, math::vec3f baseTuning, math::vec3f adaptiveTuning, math::vec3f adaptiveFiltering); + + /// \brief Generates a command to read the VPE Accelerometer Advanced Tuning register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadVpeAccelerometerAdvancedTuning(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the VPE Accelerometer Advanced Tuning register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] minFiltering The register's MinFiltering field. + /// \param[in] maxFiltering The register's MaxFiltering field. + /// \param[in] maxAdaptRate The register's MaxAdaptRate field. + /// \param[in] disturbanceWindow The register's DisturbanceWindow field. + /// \param[in] maxTuning The register's MaxTuning field. + /// \return The total number bytes in the generated command. + static size_t genWriteVpeAccelerometerAdvancedTuning(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, math::vec3f minFiltering, math::vec3f maxFiltering, float maxAdaptRate, float disturbanceWindow, float maxTuning); + + /// \brief Generates a command to read the VPE Gryo Basic Tuning register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadVpeGryoBasicTuning(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the VPE Gryo Basic Tuning register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] angularWalkVariance The register's AngularWalkVariance field. + /// \param[in] baseTuning The register's BaseTuning field. + /// \param[in] adaptiveTuning The register's AdaptiveTuning field. + /// \return The total number bytes in the generated command. + static size_t genWriteVpeGryoBasicTuning(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, math::vec3f angularWalkVariance, math::vec3f baseTuning, math::vec3f adaptiveTuning); + + /// \brief Generates a command to read the Filter Startup Gyro Bias register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadFilterStartupGyroBias(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Filter Startup Gyro Bias register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] bias The register's Bias field. + /// \return The total number bytes in the generated command. + static size_t genWriteFilterStartupGyroBias(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, math::vec3f bias); + + /// \brief Generates a command to read the Magnetometer Calibration Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadMagnetometerCalibrationControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Magnetometer Calibration Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] hsiMode The register's HSIMode field. + /// \param[in] hsiOutput The register's HSIOutput field. + /// \param[in] convergeRate The register's ConvergeRate field. + /// \return The total number bytes in the generated command. + static size_t genWriteMagnetometerCalibrationControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t hsiMode, uint8_t hsiOutput, uint8_t convergeRate); + + /// \brief Generates a command to read the Calculated Magnetometer Calibration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadCalculatedMagnetometerCalibration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the Indoor Heading Mode Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadIndoorHeadingModeControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Indoor Heading Mode Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] maxRateError The register's Max Rate Error field. + /// \return The total number bytes in the generated command. + static size_t genWriteIndoorHeadingModeControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, float maxRateError); + + /// \brief Generates a command to read the Velocity Compensation Measurement register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadVelocityCompensationMeasurement(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Velocity Compensation Measurement register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] velocity The register's Velocity field. + /// \return The total number bytes in the generated command. + static size_t genWriteVelocityCompensationMeasurement(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, math::vec3f velocity); + + /// \brief Generates a command to read the Velocity Compensation Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadVelocityCompensationControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Velocity Compensation Control register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] mode The register's Mode field. + /// \param[in] velocityTuning The register's VelocityTuning field. + /// \param[in] rateTuning The register's RateTuning field. + /// \return The total number bytes in the generated command. + static size_t genWriteVelocityCompensationControl(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t mode, float velocityTuning, float rateTuning); + + /// \brief Generates a command to read the Velocity Compensation Status register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadVelocityCompensationStatus(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the IMU Measurements register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadImuMeasurements(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the GPS Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadGpsConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the GPS Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] mode The register's Mode field. + /// \param[in] ppsSource The register's PpsSource field. + /// \return The total number bytes in the generated command. + static size_t genWriteGpsConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t mode, uint8_t ppsSource); + + /// \brief Generates a command to read the GPS Antenna Offset register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadGpsAntennaOffset(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the GPS Antenna Offset register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] position The register's Position field. + /// \return The total number bytes in the generated command. + static size_t genWriteGpsAntennaOffset(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, math::vec3f position); + + /// \brief Generates a command to read the GPS Solution - LLA register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadGpsSolutionLla(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the GPS Solution - ECEF register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadGpsSolutionEcef(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the INS Solution - LLA register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadInsSolutionLla(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the INS Solution - ECEF register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadInsSolutionEcef(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the INS Basic Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadInsBasicConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the INS Basic Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] scenario The register's Scenario field. + /// \param[in] ahrsAiding The register's AhrsAiding field. + /// \param[in] estBaseline The register's EstBaseline field. + /// \return The total number bytes in the generated command. + static size_t genWriteInsBasicConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t scenario, uint8_t ahrsAiding, uint8_t estBaseline); + + /// \brief Generates a command to read the INS Advanced Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadInsAdvancedConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the INS Advanced Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] useMag The register's UseMag field. + /// \param[in] usePres The register's UsePres field. + /// \param[in] posAtt The register's PosAtt field. + /// \param[in] velAtt The register's VelAtt field. + /// \param[in] velBias The register's VelBias field. + /// \param[in] useFoam The register's UseFoam field. + /// \param[in] gpsCovType The register's GPSCovType field. + /// \param[in] velCount The register's VelCount field. + /// \param[in] velInit The register's VelInit field. + /// \param[in] moveOrigin The register's MoveOrigin field. + /// \param[in] gpsTimeout The register's GPSTimeout field. + /// \param[in] deltaLimitPos The register's DeltaLimitPos field. + /// \param[in] deltaLimitVel The register's DeltaLimitVel field. + /// \param[in] minPosUncertainty The register's MinPosUncertainty field. + /// \param[in] minVelUncertainty The register's MinVelUncertainty field. + /// \return The total number bytes in the generated command. + static size_t genWriteInsAdvancedConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t useMag, uint8_t usePres, uint8_t posAtt, uint8_t velAtt, uint8_t velBias, uint8_t useFoam, uint8_t gpsCovType, uint8_t velCount, float velInit, float moveOrigin, float gpsTimeout, float deltaLimitPos, float deltaLimitVel, float minPosUncertainty, float minVelUncertainty); + + /// \brief Generates a command to read the INS State - LLA register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadInsStateLla(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the INS State - ECEF register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadInsStateEcef(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the Startup Filter Bias Estimate register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadStartupFilterBiasEstimate(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Startup Filter Bias Estimate register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] gyroBias The register's GyroBias field. + /// \param[in] accelBias The register's AccelBias field. + /// \param[in] pressureBias The register's PressureBias field. + /// \return The total number bytes in the generated command. + static size_t genWriteStartupFilterBiasEstimate(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, math::vec3f gyroBias, math::vec3f accelBias, float pressureBias); + + /// \brief Generates a command to read the Delta Theta and Delta Velocity register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadDeltaThetaAndDeltaVelocity(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the Delta Theta and Delta Velocity Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadDeltaThetaAndDeltaVelocityConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Delta Theta and Delta Velocity Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] integrationFrame The register's IntegrationFrame field. + /// \param[in] gyroCompensation The register's GyroCompensation field. + /// \param[in] accelCompensation The register's AccelCompensation field. + /// \return The total number bytes in the generated command. + static size_t genWriteDeltaThetaAndDeltaVelocityConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t integrationFrame, uint8_t gyroCompensation, uint8_t accelCompensation); + + /// \brief Generates a command to read the Reference Vector Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadReferenceVectorConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Reference Vector Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] useMagModel The register's UseMagModel field. + /// \param[in] useGravityModel The register's UseGravityModel field. + /// \param[in] recalcThreshold The register's RecalcThreshold field. + /// \param[in] year The register's Year field. + /// \param[in] position The register's Position field. + /// \return The total number bytes in the generated command. + static size_t genWriteReferenceVectorConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t useMagModel, uint8_t useGravityModel, uint32_t recalcThreshold, float year, math::vec3d position); + + /// \brief Generates a command to read the Gyro Compensation register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadGyroCompensation(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the Gyro Compensation register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] c The register's C field. + /// \param[in] b The register's B field. + /// \return The total number bytes in the generated command. + static size_t genWriteGyroCompensation(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, math::mat3f c, math::vec3f b); + + /// \brief Generates a command to read the IMU Filtering Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadImuFilteringConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the IMU Filtering Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] magWindowSize The register's MagWindowSize field. + /// \param[in] accelWindowSize The register's AccelWindowSize field. + /// \param[in] gyroWindowSize The register's GyroWindowSize field. + /// \param[in] tempWindowSize The register's TempWindowSize field. + /// \param[in] presWindowSize The register's PresWindowSize field. + /// \param[in] magFilterMode The register's MagFilterMode field. + /// \param[in] accelFilterMode The register's AccelFilterMode field. + /// \param[in] gyroFilterMode The register's GyroFilterMode field. + /// \param[in] tempFilterMode The register's TempFilterMode field. + /// \param[in] presFilterMode The register's PresFilterMode field. + /// \return The total number bytes in the generated command. + static size_t genWriteImuFilteringConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint16_t magWindowSize, uint16_t accelWindowSize, uint16_t gyroWindowSize, uint16_t tempWindowSize, uint16_t presWindowSize, uint8_t magFilterMode, uint8_t accelFilterMode, uint8_t gyroFilterMode, uint8_t tempFilterMode, uint8_t presFilterMode); + + /// \brief Generates a command to read the GPS Compass Baseline register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadGpsCompassBaseline(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the GPS Compass Baseline register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] position The register's Position field. + /// \param[in] uncertainty The register's Uncertainty field. + /// \return The total number bytes in the generated command. + static size_t genWriteGpsCompassBaseline(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, math::vec3f position, math::vec3f uncertainty); + + /// \brief Generates a command to read the GPS Compass Estimated Baseline register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadGpsCompassEstimatedBaseline(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the IMU Rate Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadImuRateConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to write to the IMU Rate Configuration register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \param[in] imuRate The register's imuRate field. + /// \param[in] navDivisor The register's NavDivisor field. + /// \param[in] filterTargetRate The register's filterTargetRate field. + /// \param[in] filterMinRate The register's filterMinRate field. + /// \return The total number bytes in the generated command. + static size_t genWriteImuRateConfiguration(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint16_t imuRate, uint16_t navDivisor, float filterTargetRate, float filterMinRate); + + /// \brief Generates a command to read the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadYawPitchRollTrueBodyAccelerationAndAngularRates(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \brief Generates a command to read the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register on a VectorNav sensor. + /// + /// \param[in] errorDetectionMode The type of error-detection to use in generating the command. + /// \param[in] buffer Caller provided buffer to place the generated command. + /// \param[in] size Number of bytes available in the provided buffer. + /// \return The total number bytes in the generated command. + static size_t genReadYawPitchRollTrueInertialAccelerationAndAngularRates(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size); + + /// \} + + /// \defgroup uartPacketAsciiAsyncParsers UART ASCII Asynchronous Packet Parsers + /// \brief This group of methods allow parsing of ASCII asynchronous data + /// packets from VectorNav sensors. + /// + /// The units are not specified for the out parameters since these + /// methods do a simple conversion operation from the packet string. Please + /// consult the appropriate sensor user manual for details about + /// the units returned by the sensor. + /// + /// \{ + + /// \brief Parses a VNYPR asynchronous packet. + /// + /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + void parseVNYPR(math::vec3f *yawPitchRoll); + + /// \brief Parses a VNQTN asynchronous packet. + /// + /// \param[out] quaternion The quaternion values in the packet. + void parseVNQTN(math::vec4f *quaternion); + + #ifdef INTERNAL + + /// \brief Parses a VNQTM asynchronous packet. + /// + /// \param[out] quaternion The quaternion values in the packet. + /// \param[out] magnetic The magnetic values in the packet. + void parseVNQTM(math::vec4f *quaternion, math::vec3f *magnetic); + + /// \brief Parses a VNQTA asynchronous packet. + /// + /// \param[out] quaternion The quaternion values in the packet. + /// \param[out] acceleration The acceleration values in the packet. + void parseVNQTA(math::vec4f *quaternion, math::vec3f *acceleration); + + /// \brief Parses a VNQTR asynchronous packet. + /// + /// \param[out] quaternion The quaternion values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + void parseVNQTR(math::vec4f *quaternion, math::vec3f *angularRate); + + /// \brief Parses a VNQMA asynchronous packet. + /// + /// \param[out] quaternion The quaternion values in the packet. + /// \param[out] magnetic The magnetic values in the packet. + /// \param[out] acceleration The acceleration values in the packet. + void parseVNQMA(math::vec4f *quaternion, math::vec3f *magnetic, math::vec3f *acceleration); + + /// \brief Parses a VNQAR asynchronous packet. + /// + /// \param[out] quaternion The quaternion values in the packet. + /// \param[out] acceleration The acceleration values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + void parseVNQAR(math::vec4f *quaternion, math::vec3f *acceleration, math::vec3f *angularRate); + + #endif + + /// \brief Parses a VNQMR asynchronous packet. + /// + /// \param[out] quaternion The quaternion values in the packet. + /// \param[out] magnetic The magnetic values in the packet. + /// \param[out] acceleration The acceleration values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + void parseVNQMR(math::vec4f *quaternion, math::vec3f *magnetic, math::vec3f *acceleration, math::vec3f *angularRate); + + #ifdef INTERNAL + + /// \brief Parses a VNDCM asynchronous packet. + /// + /// \param[out] dcm The directional cosine matrix values in the packet. + void parseVNDCM(math::mat3f *dcm); + + #endif + + /// \brief Parses a VNMAG asynchronous packet. + /// + /// \param[out] magnetic The magnetic values in the packet. + void parseVNMAG(math::vec3f *magnetic); + + /// \brief Parses a VNACC asynchronous packet. + /// + /// \param[out] acceleration The acceleration values in the packet. + void parseVNACC(math::vec3f *acceleration); + + /// \brief Parses a VNGYR asynchronous packet. + /// + /// \param[out] angularRate The angular rate values in the packet. + void parseVNGYR(math::vec3f *angularRate); + + /// \brief Parses a VNMAR asynchronous packet. + /// + /// \param[out] magnetic The magnetic values in the packet. + /// \param[out] acceleration The acceleration values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + void parseVNMAR(math::vec3f *magnetic, math::vec3f *acceleration, math::vec3f *angularRate); + + /// \brief Parses a VNYMR asynchronous packet. + /// + /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + /// \param[out] magnetic The magnetic values in the packet. + /// \param[out] acceleration The acceleration values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + void parseVNYMR(math::vec3f *yawPitchRoll, math::vec3f *magnetic, math::vec3f *acceleration, math::vec3f *angularRate); + + #ifdef INTERNAL + + /// \brief Parses a VNYCM asynchronous packet. + /// + /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + /// \param[out] magnetic The magnetic values in the packet. + /// \param[out] acceleration The acceleration values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + /// \param[out] temperature The temperature value in the packet. + void parseVNYCM(math::vec3f *yawPitchRoll, math::vec3f *magnetic, math::vec3f *acceleration, math::vec3f *angularRate, float *temperature); + + #endif + + /// \brief Parses a VNYBA asynchronous packet. + /// + /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + /// \param[out] accelerationBody The acceleration body values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + void parseVNYBA(math::vec3f *yawPitchRoll, math::vec3f *accelerationBody, math::vec3f *angularRate); + + /// \brief Parses a VNYIA asynchronous packet. + /// + /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + /// \param[out] accelerationInertial The acceleration inertial values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + void parseVNYIA(math::vec3f *yawPitchRoll, math::vec3f *accelerationInertial, math::vec3f *angularRate); + + #ifdef INTERNAL + + /// \brief Parses a VNICM asynchronous packet. + /// + /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + /// \param[out] magnetic The magnetic values in the packet. + /// \param[out] accelerationInertial The acceleration inertial values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + void parseVNICM(math::vec3f *yawPitchRoll, math::vec3f *magnetic, math::vec3f *accelerationInertial, math::vec3f *angularRate); + + #endif + + /// \brief Parses a VNIMU asynchronous packet. + /// + /// \param[out] magneticUncompensated The uncompensated magnetic values in the packet. + /// \param[out] accelerationUncompensated The uncompensated acceleration values in the packet. + /// \param[out] angularRateUncompensated The uncompensated angular rate values in the packet. + /// \param[out] temperature The temperature value in the packet. + /// \param[out] pressure The pressure value in the packet. + void parseVNIMU(math::vec3f *magneticUncompensated, math::vec3f *accelerationUncompensated, math::vec3f *angularRateUncompensated, float *temperature, float *pressure); + + /// \brief Parses a VNGPS asynchronous packet. + /// + /// \param[out] time The time value in the packet. + /// \param[out] week The week value in the packet. + /// \param[out] gpsFix The GPS fix value in the packet. + /// \param[out] numSats The NumSats value in the packet. + /// \param[out] lla The latitude, longitude and altitude values in the packet. + /// \param[out] nedVel The NED velocity values in the packet. + /// \param[out] nedAcc The NED position accuracy values in the packet. + /// \param[out] speedAcc The SpeedAcc value in the packet. + /// \param[out] timeAcc The TimeAcc value in the packet. + void parseVNGPS(double *time, uint16_t *week, uint8_t *gpsFix, uint8_t *numSats, math::vec3d *lla, math::vec3f *nedVel, math::vec3f *nedAcc, float *speedAcc, float *timeAcc); + + /// \brief Parses a VNINS asynchronous packet. + /// + /// \param[out] time The time value in the packet. + /// \param[out] week The week value in the packet. + /// \param[out] status The status value in the packet. + /// \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + /// \param[out] lla The latitude, longitude, altitude values in the packet. + /// \param[out] nedVel The NED velocity values in the packet. + /// \param[out] attUncertainty The attitude uncertainty value in the packet. + /// \param[out] posUncertainty The position uncertainty value in the packet. + /// \param[out] velUncertainty The velocity uncertainty value in the packet. + void parseVNINS(double *time, uint16_t *week, uint16_t *status, math::vec3f *yawPitchRoll, math::vec3d *lla, math::vec3f *nedVel, float *attUncertainty, float *posUncertainty, float *velUncertainty); + + /// \brief Parses a VNINE asynchronous packet. + /// + /// \param[out] time The time value in the packet. + /// \param[out] week The week value in the packet. + /// \param[out] status The status value in the packet. + /// \param[out] ypr The yaw, pitch, roll values in the packet. + /// \param[out] position The ECEF position values in the packet. + /// \param[out] velocity The ECEF velocity values in the packet. + /// \param[out] attUncertainty The attitude uncertainty value in the packet. + /// \param[out] posUncertainty The position uncertainty value in the packet. + /// \param[out] velUncertainty The velocity uncertainty value in the packet. + void parseVNINE(double *time, uint16_t *week, uint16_t *status, math::vec3f *ypr, math::vec3d *position, math::vec3f *velocity, float *attUncertainty, float *posUncertainty, float *velUncertainty); + + /// \brief Parses a VNISL asynchronous packet. + /// + /// \param[out] ypr The yaw, pitch, roll values in the packet. + /// \param[out] lla The latitude, longitude, altitude values in the packet. + /// \param[out] velocity The velocity values in the packet. + /// \param[out] acceleration The acceleration values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + void parseVNISL(math::vec3f* ypr, math::vec3d* lla, math::vec3f* velocity, math::vec3f* acceleration, math::vec3f* angularRate); + + /// \brief Parses a VNISE asynchronous packet. + /// + /// \param[out] ypr The yaw, pitch, roll values in the packet. + /// \param[out] position The ECEF position values in the packet. + /// \param[out] velocity The ECEF velocity values in the packet. + /// \param[out] acceleration The acceleration values in the packet. + /// \param[out] angularRate The angular rate values in the packet. + void parseVNISE(math::vec3f* ypr, math::vec3d* position, math::vec3f* velocity, math::vec3f* acceleration, math::vec3f* angularRate); + + #ifdef INTERNAL + + /// \brief Parses a VNRAW asynchronous packet. + /// + /// \param[out] magneticVoltage The magnetic voltage values in the packet. + /// \param[out] accelerationVoltage The acceleration voltage values in the packet. + /// \param[out] angularRateVoltage The angular rate voltage values in the packet. + /// \param[out] temperatureVoltage The temperature voltage value in the packet. + void parseVNRAW(math::vec3f *magneticVoltage, math::vec3f *accelerationVoltage, math::vec3f *angularRateVoltage, float *temperatureVoltage); + + /// \brief Parses a VNCMV asynchronous packet. + /// + /// \param[out] magneticUncompensated The uncompensated magnetic values in the packet. + /// \param[out] accelerationUncompensated The uncompensated acceleration values in the packet. + /// \param[out] angularRateUncompensated The uncompensated angular rate values in the packet. + /// \param[out] temperature The temperature value in the packet. + void parseVNCMV(math::vec3f *magneticUncompensated, math::vec3f *accelerationUncompensated, math::vec3f *angularRateUncompensated, float *temperature); + + /// \brief Parses a VNSTV asynchronous packet. + /// + /// \param[out] quaternion The quaternion values in the packet. + /// \param[out] angularRateBias The angular rate bias values in the packet. + void parseVNSTV(math::vec4f *quaternion, math::vec3f *angularRateBias); + + /// \brief Parses a VNCOV asynchronous packet. + /// + /// \param[out] attitudeVariance The attitude variance values in the packet. + /// \param[out] angularRateBiasVariance The angular rate bias variance values in the packet. + void parseVNCOV(math::vec3f *attitudeVariance, math::vec3f *angularRateBiasVariance); + + #endif + + /// \brief Parses a VNGPE asynchronous packet. + /// + /// \param[out] tow The tow value in the packet. + /// \param[out] week The week value in the packet. + /// \param[out] gpsFix The GPS fix value in the packet. + /// \param[out] numSats The numSats value in the packet. + /// \param[out] position The ECEF position values in the packet. + /// \param[out] velocity The ECEF velocity values in the packet. + /// \param[out] posAcc The PosAcc values in the packet. + /// \param[out] speedAcc The SpeedAcc value in the packet. + /// \param[out] timeAcc The TimeAcc value in the packet. + void parseVNGPE(double *tow, uint16_t *week, uint8_t *gpsFix, uint8_t *numSats, math::vec3d *position, math::vec3f *velocity, math::vec3f *posAcc, float *speedAcc, float *timeAcc); + + /// \brief Parses a VNDTV asynchronous packet. + /// + /// \param[out] deltaTime The DeltaTime value in the packet. + /// \param[out] deltaTheta The DeltaTheta values in the packet. + /// \param[out] deltaVelocity The DeltaVelocity values in the packet. + void parseVNDTV(float *deltaTime, math::vec3f *deltaTheta, math::vec3f *deltaVelocity); + + /// \} + + /// \defgroup uartAsciiResponseParsers UART ASCII Response Packet Parsers + /// \brief This group of methods allow parsing of ASCII response data + /// packets from VectorNav's sensors. + /// + /// The units are not specified for the out parameters since these + /// methods do a simple conversion operation from the packet string. Please + /// consult the appropriate user manual for your sensor for details about + /// the units returned by the sensor. + /// + /// \{ + + /// \brief Parses a response from reading any of the Binary Output registers. + /// + /// \param[out] asyncMode The register's AsyncMode field. + /// \param[out] rateDivisor The register's RateDivisor field. + /// \param[out] outputGroup The register's OutputGroup field. + /// \param[out] commonField The set fields of Output Group 1 (Common) if present. + /// \param[out] timeField The set fields of Output Group 2 (Time) if present. + /// \param[out] imuField The set fields of Output Group 3 (IMU) if present. + /// \param[out] gpsField The set fields of Output Group 4 (GPS) if present. + /// \param[out] attitudeField The set fields of Output Group 5 (Attitude) if present. + /// \param[out] insField The set fields of Output Group 6 (INS) if present. + void parseBinaryOutput( + uint16_t* asyncMode, + uint16_t* rateDivisor, + uint16_t* outputGroup, + uint16_t* commonField, + uint16_t* timeField, + uint16_t* imuField, + uint16_t* gpsField, + uint16_t* attitudeField, + uint16_t* insField); + + /// \brief Parses a response from reading the User Tag register. + /// + /// \param[out] tag The register's Tag field. + void parseUserTag(char* tag); + + /// \brief Parses a response from reading the Model Number register. + /// + /// \param[out] productName The register's Product Name field. + void parseModelNumber(char* productName); + + /// \brief Parses a response from reading the Hardware Revision register. + /// + /// \param[out] revision The register's Revision field. + void parseHardwareRevision(uint32_t* revision); + + /// \brief Parses a response from reading the Serial Number register. + /// + /// \param[out] serialNum The register's SerialNum field. + void parseSerialNumber(uint32_t* serialNum); + + /// \brief Parses a response from reading the Firmware Version register. + /// + /// \param[out] firmwareVersion The register's Firmware Version field. + void parseFirmwareVersion(char* firmwareVersion); + + /// \brief Parses a response from reading the Serial Baud Rate register. + /// + /// \param[out] baudrate The register's Baud Rate field. + void parseSerialBaudRate(uint32_t* baudrate); + + /// \brief Parses a response from reading the Async Data Output Type register. + /// + /// \param[out] ador The register's ADOR field. + void parseAsyncDataOutputType(uint32_t* ador); + + /// \brief Parses a response from reading the Async Data Output Frequency register. + /// + /// \param[out] adof The register's ADOF field. + void parseAsyncDataOutputFrequency(uint32_t* adof); + + /// \brief Parses a response from reading the Yaw Pitch Roll register. + /// + /// \param[out] yawPitchRoll The register's YawPitchRoll field. + void parseYawPitchRoll(math::vec3f* yawPitchRoll); + + /// \brief Parses a response from reading the Attitude Quaternion register. + /// + /// \param[out] quat The register's Quat field. + void parseAttitudeQuaternion(math::vec4f* quat); + + /// \brief Parses a response from reading the Quaternion, Magnetic, Acceleration and Angular Rates register. + /// + /// \param[out] quat The register's Quat field. + /// \param[out] mag The register's Mag field. + /// \param[out] accel The register's Accel field. + /// \param[out] gyro The register's Gyro field. + void parseQuaternionMagneticAccelerationAndAngularRates(math::vec4f* quat, math::vec3f* mag, math::vec3f* accel, math::vec3f* gyro); + + /// \brief Parses a response from reading the Magnetic Measurements register. + /// + /// \param[out] mag The register's Mag field. + void parseMagneticMeasurements(math::vec3f* mag); + + /// \brief Parses a response from reading the Acceleration Measurements register. + /// + /// \param[out] accel The register's Accel field. + void parseAccelerationMeasurements(math::vec3f* accel); + + /// \brief Parses a response from reading the Angular Rate Measurements register. + /// + /// \param[out] gyro The register's Gyro field. + void parseAngularRateMeasurements(math::vec3f* gyro); + + /// \brief Parses a response from reading the Magnetic, Acceleration and Angular Rates register. + /// + /// \param[out] mag The register's Mag field. + /// \param[out] accel The register's Accel field. + /// \param[out] gyro The register's Gyro field. + void parseMagneticAccelerationAndAngularRates(math::vec3f* mag, math::vec3f* accel, math::vec3f* gyro); + + /// \brief Parses a response from reading the Magnetic and Gravity Reference Vectors register. + /// + /// \param[out] magRef The register's MagRef field. + /// \param[out] accRef The register's AccRef field. + void parseMagneticAndGravityReferenceVectors(math::vec3f* magRef, math::vec3f* accRef); + + /// \brief Parses a response from reading the Filter Measurements Variance Parameters register. + /// + /// \param[out] angularWalkVariance The register's Angular Walk Variance field. + /// \param[out] angularRateVariance The register's Angular Rate Variance field. + /// \param[out] magneticVariance The register's Magnetic Variance field. + /// \param[out] accelerationVariance The register's Acceleration Variance field. + void parseFilterMeasurementsVarianceParameters(float* angularWalkVariance, math::vec3f* angularRateVariance, math::vec3f* magneticVariance, math::vec3f* accelerationVariance); + + /// \brief Parses a response from reading the Magnetometer Compensation register. + /// + /// \param[out] c The register's C field. + /// \param[out] b The register's B field. + void parseMagnetometerCompensation(math::mat3f* c, math::vec3f* b); + + /// \brief Parses a response from reading the Filter Active Tuning Parameters register. + /// + /// \param[out] magneticDisturbanceGain The register's Magnetic Disturbance Gain field. + /// \param[out] accelerationDisturbanceGain The register's Acceleration Disturbance Gain field. + /// \param[out] magneticDisturbanceMemory The register's Magnetic Disturbance Memory field. + /// \param[out] accelerationDisturbanceMemory The register's Acceleration Disturbance Memory field. + void parseFilterActiveTuningParameters(float* magneticDisturbanceGain, float* accelerationDisturbanceGain, float* magneticDisturbanceMemory, float* accelerationDisturbanceMemory); + + /// \brief Parses a response from reading the Acceleration Compensation register. + /// + /// \param[out] c The register's C field. + /// \param[out] b The register's B field. + void parseAccelerationCompensation(math::mat3f* c, math::vec3f* b); + + /// \brief Parses a response from reading the Reference Frame Rotation register. + /// + /// \param[out] c The register's C field. + void parseReferenceFrameRotation(math::mat3f* c); + + /// \brief Parses a response from reading the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register. + /// + /// \param[out] yawPitchRoll The register's YawPitchRoll field. + /// \param[out] mag The register's Mag field. + /// \param[out] accel The register's Accel field. + /// \param[out] gyro The register's Gyro field. + void parseYawPitchRollMagneticAccelerationAndAngularRates(math::vec3f* yawPitchRoll, math::vec3f* mag, math::vec3f* accel, math::vec3f* gyro); + + /// \brief Parses a response from reading the Communication Protocol Control register. + /// + /// \param[out] serialCount The register's SerialCount field. + /// \param[out] serialStatus The register's SerialStatus field. + /// \param[out] spiCount The register's SPICount field. + /// \param[out] spiStatus The register's SPIStatus field. + /// \param[out] serialChecksum The register's SerialChecksum field. + /// \param[out] spiChecksum The register's SPIChecksum field. + /// \param[out] errorMode The register's ErrorMode field. + void parseCommunicationProtocolControl(uint8_t* serialCount, uint8_t* serialStatus, uint8_t* spiCount, uint8_t* spiStatus, uint8_t* serialChecksum, uint8_t* spiChecksum, uint8_t* errorMode); + + /// \brief Parses a response from reading the Synchronization Control register. + /// + /// \param[out] syncInMode The register's SyncInMode field. + /// \param[out] syncInEdge The register's SyncInEdge field. + /// \param[out] syncInSkipFactor The register's SyncInSkipFactor field. + /// \param[out] syncOutMode The register's SyncOutMode field. + /// \param[out] syncOutPolarity The register's SyncOutPolarity field. + /// \param[out] syncOutSkipFactor The register's SyncOutSkipFactor field. + /// \param[out] syncOutPulseWidth The register's SyncOutPulseWidth field. + void parseSynchronizationControl(uint8_t* syncInMode, uint8_t* syncInEdge, uint16_t* syncInSkipFactor, uint8_t* syncOutMode, uint8_t* syncOutPolarity, uint16_t* syncOutSkipFactor, uint32_t* syncOutPulseWidth); + + /// \brief Parses a response from reading the Synchronization Status register. + /// + /// \param[out] syncInCount The register's SyncInCount field. + /// \param[out] syncInTime The register's SyncInTime field. + /// \param[out] syncOutCount The register's SyncOutCount field. + void parseSynchronizationStatus(uint32_t* syncInCount, uint32_t* syncInTime, uint32_t* syncOutCount); + + /// \brief Parses a response from reading the Filter Basic Control register. + /// + /// \param[out] magMode The register's MagMode field. + /// \param[out] extMagMode The register's ExtMagMode field. + /// \param[out] extAccMode The register's ExtAccMode field. + /// \param[out] extGyroMode The register's ExtGyroMode field. + /// \param[out] gyroLimit The register's GyroLimit field. + void parseFilterBasicControl(uint8_t* magMode, uint8_t* extMagMode, uint8_t* extAccMode, uint8_t* extGyroMode, math::vec3f* gyroLimit); + + /// \brief Parses a response from reading the VPE Basic Control register. + /// + /// \param[out] enable The register's Enable field. + /// \param[out] headingMode The register's HeadingMode field. + /// \param[out] filteringMode The register's FilteringMode field. + /// \param[out] tuningMode The register's TuningMode field. + void parseVpeBasicControl(uint8_t* enable, uint8_t* headingMode, uint8_t* filteringMode, uint8_t* tuningMode); + + /// \brief Parses a response from reading the VPE Magnetometer Basic Tuning register. + /// + /// \param[out] baseTuning The register's BaseTuning field. + /// \param[out] adaptiveTuning The register's AdaptiveTuning field. + /// \param[out] adaptiveFiltering The register's AdaptiveFiltering field. + void parseVpeMagnetometerBasicTuning(math::vec3f* baseTuning, math::vec3f* adaptiveTuning, math::vec3f* adaptiveFiltering); + + /// \brief Parses a response from reading the VPE Magnetometer Advanced Tuning register. + /// + /// \param[out] minFiltering The register's MinFiltering field. + /// \param[out] maxFiltering The register's MaxFiltering field. + /// \param[out] maxAdaptRate The register's MaxAdaptRate field. + /// \param[out] disturbanceWindow The register's DisturbanceWindow field. + /// \param[out] maxTuning The register's MaxTuning field. + void parseVpeMagnetometerAdvancedTuning(math::vec3f* minFiltering, math::vec3f* maxFiltering, float* maxAdaptRate, float* disturbanceWindow, float* maxTuning); + + /// \brief Parses a response from reading the VPE Accelerometer Basic Tuning register. + /// + /// \param[out] baseTuning The register's BaseTuning field. + /// \param[out] adaptiveTuning The register's AdaptiveTuning field. + /// \param[out] adaptiveFiltering The register's AdaptiveFiltering field. + void parseVpeAccelerometerBasicTuning(math::vec3f* baseTuning, math::vec3f* adaptiveTuning, math::vec3f* adaptiveFiltering); + + /// \brief Parses a response from reading the VPE Accelerometer Advanced Tuning register. + /// + /// \param[out] minFiltering The register's MinFiltering field. + /// \param[out] maxFiltering The register's MaxFiltering field. + /// \param[out] maxAdaptRate The register's MaxAdaptRate field. + /// \param[out] disturbanceWindow The register's DisturbanceWindow field. + /// \param[out] maxTuning The register's MaxTuning field. + void parseVpeAccelerometerAdvancedTuning(math::vec3f* minFiltering, math::vec3f* maxFiltering, float* maxAdaptRate, float* disturbanceWindow, float* maxTuning); + + /// \brief Parses a response from reading the VPE Gryo Basic Tuning register. + /// + /// \param[out] angularWalkVariance The register's AngularWalkVariance field. + /// \param[out] baseTuning The register's BaseTuning field. + /// \param[out] adaptiveTuning The register's AdaptiveTuning field. + void parseVpeGryoBasicTuning(math::vec3f* angularWalkVariance, math::vec3f* baseTuning, math::vec3f* adaptiveTuning); + + /// \brief Parses a response from reading the Filter Startup Gyro Bias register. + /// + /// \param[out] bias The register's Bias field. + void parseFilterStartupGyroBias(math::vec3f* bias); + + /// \brief Parses a response from reading the Magnetometer Calibration Control register. + /// + /// \param[out] hsiMode The register's HSIMode field. + /// \param[out] hsiOutput The register's HSIOutput field. + /// \param[out] convergeRate The register's ConvergeRate field. + void parseMagnetometerCalibrationControl(uint8_t* hsiMode, uint8_t* hsiOutput, uint8_t* convergeRate); + + /// \brief Parses a response from reading the Calculated Magnetometer Calibration register. + /// + /// \param[out] c The register's C field. + /// \param[out] b The register's B field. + void parseCalculatedMagnetometerCalibration(math::mat3f* c, math::vec3f* b); + + /// \brief Parses a response from reading the Indoor Heading Mode Control register. + /// + /// \param[out] maxRateError The register's Max Rate Error field. + void parseIndoorHeadingModeControl(float* maxRateError); + + /// \brief Parses a response from reading the Velocity Compensation Measurement register. + /// + /// \param[out] velocity The register's Velocity field. + void parseVelocityCompensationMeasurement(math::vec3f* velocity); + + /// \brief Parses a response from reading the Velocity Compensation Control register. + /// + /// \param[out] mode The register's Mode field. + /// \param[out] velocityTuning The register's VelocityTuning field. + /// \param[out] rateTuning The register's RateTuning field. + void parseVelocityCompensationControl(uint8_t* mode, float* velocityTuning, float* rateTuning); + + /// \brief Parses a response from reading the Velocity Compensation Status register. + /// + /// \param[out] x The register's x field. + /// \param[out] xDot The register's xDot field. + /// \param[out] accelOffset The register's accelOffset field. + /// \param[out] omega The register's omega field. + void parseVelocityCompensationStatus(float* x, float* xDot, math::vec3f* accelOffset, math::vec3f* omega); + + /// \brief Parses a response from reading the IMU Measurements register. + /// + /// \param[out] mag The register's Mag field. + /// \param[out] accel The register's Accel field. + /// \param[out] gyro The register's Gyro field. + /// \param[out] temp The register's Temp field. + /// \param[out] pressure The register's Pressure field. + void parseImuMeasurements(math::vec3f* mag, math::vec3f* accel, math::vec3f* gyro, float* temp, float* pressure); + + /// \brief Parses a response from reading the GPS Configuration register. + /// + /// \param[out] mode The register's Mode field. + /// \param[out] ppsSource The register's PpsSource field. + void parseGpsConfiguration(uint8_t* mode, uint8_t* ppsSource); + + /// \brief Parses a response from reading the GPS Antenna Offset register. + /// + /// \param[out] position The register's Position field. + void parseGpsAntennaOffset(math::vec3f* position); + + /// \brief Parses a response from reading the GPS Solution - LLA register. + /// + /// \param[out] time The register's Time field. + /// \param[out] week The register's Week field. + /// \param[out] gpsFix The register's GpsFix field. + /// \param[out] numSats The register's NumSats field. + /// \param[out] lla The register's Lla field. + /// \param[out] nedVel The register's NedVel field. + /// \param[out] nedAcc The register's NedAcc field. + /// \param[out] speedAcc The register's SpeedAcc field. + /// \param[out] timeAcc The register's TimeAcc field. + void parseGpsSolutionLla(double* time, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, math::vec3d* lla, math::vec3f* nedVel, math::vec3f* nedAcc, float* speedAcc, float* timeAcc); + + /// \brief Parses a response from reading the GPS Solution - ECEF register. + /// + /// \param[out] tow The register's Tow field. + /// \param[out] week The register's Week field. + /// \param[out] gpsFix The register's GpsFix field. + /// \param[out] numSats The register's NumSats field. + /// \param[out] position The register's Position field. + /// \param[out] velocity The register's Velocity field. + /// \param[out] posAcc The register's PosAcc field. + /// \param[out] speedAcc The register's SpeedAcc field. + /// \param[out] timeAcc The register's TimeAcc field. + void parseGpsSolutionEcef(double* tow, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, math::vec3d* position, math::vec3f* velocity, math::vec3f* posAcc, float* speedAcc, float* timeAcc); + + /// \brief Parses a response from reading the INS Solution - LLA register. + /// + /// \param[out] time The register's Time field. + /// \param[out] week The register's Week field. + /// \param[out] status The register's Status field. + /// \param[out] yawPitchRoll The register's YawPitchRoll field. + /// \param[out] position The register's Position field. + /// \param[out] nedVel The register's NedVel field. + /// \param[out] attUncertainty The register's AttUncertainty field. + /// \param[out] posUncertainty The register's PosUncertainty field. + /// \param[out] velUncertainty The register's VelUncertainty field. + void parseInsSolutionLla(double* time, uint16_t* week, uint16_t* status, math::vec3f* yawPitchRoll, math::vec3d* position, math::vec3f* nedVel, float* attUncertainty, float* posUncertainty, float* velUncertainty); + + /// \brief Parses a response from reading the INS Solution - ECEF register. + /// + /// \param[out] time The register's Time field. + /// \param[out] week The register's Week field. + /// \param[out] status The register's Status field. + /// \param[out] yawPitchRoll The register's YawPitchRoll field. + /// \param[out] position The register's Position field. + /// \param[out] velocity The register's Velocity field. + /// \param[out] attUncertainty The register's AttUncertainty field. + /// \param[out] posUncertainty The register's PosUncertainty field. + /// \param[out] velUncertainty The register's VelUncertainty field. + void parseInsSolutionEcef(double* time, uint16_t* week, uint16_t* status, math::vec3f* yawPitchRoll, math::vec3d* position, math::vec3f* velocity, float* attUncertainty, float* posUncertainty, float* velUncertainty); + + /// \brief Parses a response from reading the INS Basic Configuration register. + /// + /// \param[out] scenario The register's Scenario field. + /// \param[out] ahrsAiding The register's AhrsAiding field. + void parseInsBasicConfiguration(uint8_t* scenario, uint8_t* ahrsAiding); + + /// \brief Parses a response from reading the INS Basic Configuration register. + /// + /// \param[out] scenario The register's Scenario field. + /// \param[out] ahrsAiding The register's AhrsAiding field. + /// \param[out] estBaseline The register's EstBaseline field. + void parseInsBasicConfiguration(uint8_t* scenario, uint8_t* ahrsAiding, uint8_t* estBaseline); + + /// \brief Parses a response from reading the INS Advanced Configuration register. + /// + /// \param[out] useMag The register's UseMag field. + /// \param[out] usePres The register's UsePres field. + /// \param[out] posAtt The register's PosAtt field. + /// \param[out] velAtt The register's VelAtt field. + /// \param[out] velBias The register's VelBias field. + /// \param[out] useFoam The register's UseFoam field. + /// \param[out] gpsCovType The register's GPSCovType field. + /// \param[out] velCount The register's VelCount field. + /// \param[out] velInit The register's VelInit field. + /// \param[out] moveOrigin The register's MoveOrigin field. + /// \param[out] gpsTimeout The register's GPSTimeout field. + /// \param[out] deltaLimitPos The register's DeltaLimitPos field. + /// \param[out] deltaLimitVel The register's DeltaLimitVel field. + /// \param[out] minPosUncertainty The register's MinPosUncertainty field. + /// \param[out] minVelUncertainty The register's MinVelUncertainty field. + void parseInsAdvancedConfiguration(uint8_t* useMag, uint8_t* usePres, uint8_t* posAtt, uint8_t* velAtt, uint8_t* velBias, uint8_t* useFoam, uint8_t* gpsCovType, uint8_t* velCount, float* velInit, float* moveOrigin, float* gpsTimeout, float* deltaLimitPos, float* deltaLimitVel, float* minPosUncertainty, float* minVelUncertainty); + + /// \brief Parses a response from reading the INS State - LLA register. + /// + /// \param[out] yawPitchRoll The register's YawPitchRoll field. + /// \param[out] position The register's Position field. + /// \param[out] velocity The register's Velocity field. + /// \param[out] accel The register's Accel field. + /// \param[out] angularRate The register's AngularRate field. + void parseInsStateLla(math::vec3f* yawPitchRoll, math::vec3d* position, math::vec3f* velocity, math::vec3f* accel, math::vec3f* angularRate); + + /// \brief Parses a response from reading the INS State - ECEF register. + /// + /// \param[out] yawPitchRoll The register's YawPitchRoll field. + /// \param[out] position The register's Position field. + /// \param[out] velocity The register's Velocity field. + /// \param[out] accel The register's Accel field. + /// \param[out] angularRate The register's AngularRate field. + void parseInsStateEcef(math::vec3f* yawPitchRoll, math::vec3d* position, math::vec3f* velocity, math::vec3f* accel, math::vec3f* angularRate); + + /// \brief Parses a response from reading the Startup Filter Bias Estimate register. + /// + /// \param[out] gyroBias The register's GyroBias field. + /// \param[out] accelBias The register's AccelBias field. + /// \param[out] pressureBias The register's PressureBias field. + void parseStartupFilterBiasEstimate(math::vec3f* gyroBias, math::vec3f* accelBias, float* pressureBias); + + /// \brief Parses a response from reading the Delta Theta and Delta Velocity register. + /// + /// \param[out] deltaTime The register's DeltaTime field. + /// \param[out] deltaTheta The register's DeltaTheta field. + /// \param[out] deltaVelocity The register's DeltaVelocity field. + void parseDeltaThetaAndDeltaVelocity(float* deltaTime, math::vec3f* deltaTheta, math::vec3f* deltaVelocity); + + /// \brief Parses a response from reading the Delta Theta and Delta Velocity Configuration register. + /// + /// \param[out] integrationFrame The register's IntegrationFrame field. + /// \param[out] gyroCompensation The register's GyroCompensation field. + /// \param[out] accelCompensation The register's AccelCompensation field. + void parseDeltaThetaAndDeltaVelocityConfiguration(uint8_t* integrationFrame, uint8_t* gyroCompensation, uint8_t* accelCompensation); + + /// \brief Parses a response from reading the Reference Vector Configuration register. + /// + /// \param[out] useMagModel The register's UseMagModel field. + /// \param[out] useGravityModel The register's UseGravityModel field. + /// \param[out] recalcThreshold The register's RecalcThreshold field. + /// \param[out] year The register's Year field. + /// \param[out] position The register's Position field. + void parseReferenceVectorConfiguration(uint8_t* useMagModel, uint8_t* useGravityModel, uint32_t* recalcThreshold, float* year, math::vec3d* position); + + /// \brief Parses a response from reading the Gyro Compensation register. + /// + /// \param[out] c The register's C field. + /// \param[out] b The register's B field. + void parseGyroCompensation(math::mat3f* c, math::vec3f* b); + + /// \brief Parses a response from reading the IMU Filtering Configuration register. + /// + /// \param[out] magWindowSize The register's MagWindowSize field. + /// \param[out] accelWindowSize The register's AccelWindowSize field. + /// \param[out] gyroWindowSize The register's GyroWindowSize field. + /// \param[out] tempWindowSize The register's TempWindowSize field. + /// \param[out] presWindowSize The register's PresWindowSize field. + /// \param[out] magFilterMode The register's MagFilterMode field. + /// \param[out] accelFilterMode The register's AccelFilterMode field. + /// \param[out] gyroFilterMode The register's GyroFilterMode field. + /// \param[out] tempFilterMode The register's TempFilterMode field. + /// \param[out] presFilterMode The register's PresFilterMode field. + void parseImuFilteringConfiguration(uint16_t* magWindowSize, uint16_t* accelWindowSize, uint16_t* gyroWindowSize, uint16_t* tempWindowSize, uint16_t* presWindowSize, uint8_t* magFilterMode, uint8_t* accelFilterMode, uint8_t* gyroFilterMode, uint8_t* tempFilterMode, uint8_t* presFilterMode); + + /// \brief Parses a response from reading the GPS Compass Baseline register. + /// + /// \param[out] position The register's Position field. + /// \param[out] uncertainty The register's Uncertainty field. + void parseGpsCompassBaseline(math::vec3f* position, math::vec3f* uncertainty); + + /// \brief Parses a response from reading the GPS Compass Estimated Baseline register. + /// + /// \param[out] estBaselineUsed The register's EstBaselineUsed field. + /// \param[out] numMeas The register's NumMeas field. + /// \param[out] position The register's Position field. + /// \param[out] uncertainty The register's Uncertainty field. + void parseGpsCompassEstimatedBaseline(uint8_t* estBaselineUsed, uint16_t* numMeas, math::vec3f* position, math::vec3f* uncertainty); + + /// \brief Parses a response from reading the IMU Rate Configuration register. + /// + /// \param[out] imuRate The register's imuRate field. + /// \param[out] navDivisor The register's NavDivisor field. + /// \param[out] filterTargetRate The register's filterTargetRate field. + /// \param[out] filterMinRate The register's filterMinRate field. + void parseImuRateConfiguration(uint16_t* imuRate, uint16_t* navDivisor, float* filterTargetRate, float* filterMinRate); + + /// \brief Parses a response from reading the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register. + /// + /// \param[out] yawPitchRoll The register's YawPitchRoll field. + /// \param[out] bodyAccel The register's BodyAccel field. + /// \param[out] gyro The register's Gyro field. + void parseYawPitchRollTrueBodyAccelerationAndAngularRates(math::vec3f* yawPitchRoll, math::vec3f* bodyAccel, math::vec3f* gyro); + + /// \brief Parses a response from reading the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register. + /// + /// \param[out] yawPitchRoll The register's YawPitchRoll field. + /// \param[out] inertialAccel The register's InertialAccel field. + /// \param[out] gyro The register's Gyro field. + void parseYawPitchRollTrueInertialAccelerationAndAngularRates(math::vec3f* yawPitchRoll, math::vec3f* inertialAccel, math::vec3f* gyro); + + /// \} + +private: + + void ensureCanExtract(size_t numOfBytes); + + bool _isPacketDataMine; + size_t _length; + char *_data; + size_t _curExtractLoc; +}; + +} +} +} + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/protocol/uart/packetfinder.h b/ext/vnproglib-1.1.0.115/include/vn/protocol/uart/packetfinder.h new file mode 100755 index 0000000..9998007 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/protocol/uart/packetfinder.h @@ -0,0 +1,104 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifndef _VNPROTOCOL_UART_PACKETFINDER_H_ +#define _VNPROTOCOL_UART_PACKETFINDER_H_ + +#include "vn/util/nocopy.h" +#include "vn/xplat/time.h" +#include "vn/protocol/uart/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); + + /// \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, 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 diff --git a/ext/vnproglib-1.1.0.115/include/vn/protocol/uart/types.h b/ext/vnproglib-1.1.0.115/include/vn/protocol/uart/types.h new file mode 100755 index 0000000..41fa85a --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/protocol/uart/types.h @@ -0,0 +1,612 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifndef _VNPROTOCOL_UART_TYPES_H_ +#define _VNPROTOCOL_UART_TYPES_H_ + +#include "vn/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. + #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. +}; + +/// \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. +}; + +/// \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' 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. +}; + +/// \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. +}; + +/// \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. +}; + +/// \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 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 Compensation modes for the Delta Theta and Delta Velocity configuration register. +enum CompensationMode +{ + /// \brief None. + COMPENSATIONMODE_NONE = 0, + /// \brief Bias. + COMPENSATIONMODE_BIAS = 1 +}; + +/// \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 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 VpeStatus from the raw bit field received + /// from the sensor. + explicit VpeStatus(uint16_t raw); +}; + +/// \brief Status flags for INS filters. +enum InsStatus +{ + INSSTATUS_NOT_TRACKING = 0x00, ///< \brief Not tracking. + INSSTATUS_SUFFICIENT_DYNAMIC_MOTIONn = 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. + uint8_t ms; ///< \brief Ms field. + + /// \brief Default constructor. + //TimeUtc() { } +}; + +/// \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 diff --git a/ext/vnproglib-1.1.0.115/include/vn/protocol/uart/util.h b/ext/vnproglib-1.1.0.115/include/vn/protocol/uart/util.h new file mode 100755 index 0000000..38ff2b2 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/protocol/uart/util.h @@ -0,0 +1,383 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifndef _VNPROTOCOL_UART_UTIL_H_ +#define _VNPROTOCOL_UART_UTIL_H_ + +#include +#include "vn/protocol/uart/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 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 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 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 +/// 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 +/// 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 diff --git a/ext/vnproglib-1.1.0.115/include/vn/python/util.h b/ext/vnproglib-1.1.0.115/include/vn/python/util.h new file mode 100755 index 0000000..111ffde --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/python/util.h @@ -0,0 +1,83 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#pragma once + +#include "Python.h" + +namespace vn { +namespace python { + +/// \brief Used for releasing the Python Global Interpretor Lock. +/// +/// +/// void run() +/// { +/// // Constructor does necessary unlocking. +/// ReleaseGIL gilUnlocked = ReleaseGIL(); +/// +/// // ... do your stuff here ... +/// +/// // Once function returns, destructor takes care for reaquiring the lock. +/// } +/// +/// +/// NOTE: Need to ensure that PyEval_InitThreads() is called early in the program +/// before use this class. +class ReleaseGIL +{ +public: + inline ReleaseGIL() + { + // Python threads must have already been initialized globally via PyEval_InitThreads(). + //assert(PyEval_ThreadsInitialize()); + + _saveState = PyEval_SaveThread(); + } + + inline ~ReleaseGIL() + { + PyEval_RestoreThread(_saveState); + } + +private: + PyThreadState* _saveState; +}; + +/// \brief Used for acquiring and then releasing the Python Global Interpretor Lock. +/// +/// +/// void run() +/// { +/// // Constructor does necessary locking. +/// AcquireGIL gilLocked; +/// +/// // ... do your stuff here such as calling Python code ... +/// +/// // Once function returns, destructor takes care for releasing the lock. +/// } +/// +/// +/// NOTE: Need to ensure that PyEval_InitThreads() is called early in the program +/// before use this class. +class AcquireGIL +{ +public: + inline AcquireGIL() + { + // Python threads must have already been initialized globally via PyEval_InitThreads(). + //assert(PyEval_ThreadsInitialize()); + + _saveState = PyGILState_Ensure(); + } + + inline ~AcquireGIL() + { + PyGILState_Release(_saveState); + } + +private: + PyGILState_STATE _saveState; +}; + +} +} diff --git a/ext/vnproglib-1.1.0.115/include/vn/sensors/compositedata.h b/ext/vnproglib-1.1.0.115/include/vn/sensors/compositedata.h new file mode 100755 index 0000000..a684a75 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/sensors/compositedata.h @@ -0,0 +1,669 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifndef _VNSENSORS_COMPOSITEDATA_H_ +#define _VNSENSORS_COMPOSITEDATA_H_ + +#include + +#include "vn/util/export.h" +#include "vn/protocol/uart/packet.h" +#include "vn/math/attitude.h" +#include "vn/math/position.h" + +namespace vn { +namespace sensors { + +/// \brief Composite structure of all data types available from VectorNav sensors. +class vn_proglib_DLLEXPORT CompositeData +{ +public: + + CompositeData(); + + /// \brief Parses a packet. + /// + /// \param[in] p The packet to parse. + /// \return The data contained in the parsed packet. + static CompositeData parse(protocol::uart::Packet& p); + + /// \brief Parses a packet. + /// + /// \param[in] p The packet to parse. + /// \param[in/out] o The CompositeData structure to write the data to. + static void parse(protocol::uart::Packet& p, CompositeData& o); + + /// \brief Parses a packet and updates multiple CompositeData objects. + /// + /// \param[in] p The packet to parse. + /// \param[in] o The collection of CompositeData objects to update. + static void parse(protocol::uart::Packet& p, std::vector& o); + + /// \brief Resets the data contained in the CompositeData object. + void reset(); + + /// \brief Indicates if anyAttitude has valid data. + /// \return true if anyAttitude has valid data; otherwise false. + bool hasAnyAttitude(); + + /// \brief Gets and converts the latest attitude data regardless of the received + /// underlying type. Based on which type of attitude data that was last received + /// and processed, this value may be based on either received yaw, pitch, roll, + /// quaternion, or direction cosine matrix data. + /// + /// \return The attitude data. + /// \exception invalid_operation Thrown if there is no valid data. + math::AttitudeF anyAttitude(); + + /// \brief Indicates if yawPitchRoll has valid data. + /// \return true if yawPitchRoll has valid data; otherwise false. + bool hasYawPitchRoll(); + + /// \brief Yaw, pitch, roll data. + /// \return The yaw, pitch, roll data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f yawPitchRoll(); + + /// \brief Indicates if quaternion has valid data. + /// \return true if quaternion has valid data; otherwise false. + bool hasQuaternion(); + + /// \brief Quaternion data. + /// \return Quaternion data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec4f quaternion(); + + /// \brief Indicates if directionCosineMatrix has valid data. + /// \return true if directionCosineMatrix has valid data; otherwise false. + bool hasDirectionCosineMatrix(); + + /// \brief Direction cosine matrix data. + /// \return Direction cosine matrix data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::mat3f directionCosineMatrix(); + + /// \brief Indicates if anyMagnetic has valid data. + /// \return true if anyMagnetic has valid data; otherwise false. + bool hasAnyMagnetic(); + + /// \brief Gets and converts the latest magnetic data regardless of the received + /// underlying type. Based on which type of magnetic data that was last received + /// and processed, this value may be based on either received magnetic or + /// magneticUncompensated data. + /// + /// \return The magnetic data. + /// \exception invalid_operation Thrown if there is no valid data. + math::vec3f anyMagnetic(); + + /// \brief Indicates if magnetic has valid data. + /// \return true if magnetic has valid data; otherwise false. + bool hasMagnetic(); + + /// \brief Magnetic data. + /// \return The magnetic data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f magnetic(); + + /// \brief Indicates if magneticUncompensated has valid data. + /// \return true if magneticUncompensated has valid data; otherwise false. + bool hasMagneticUncompensated(); + + /// \brief Magnetic uncompensated data. + /// \return The magnetic uncompensated data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f magneticUncompensated(); + + /// \brief Indicates if magneticNed has valid data. + /// \return true if magneticNed has valid data; otherwise false. + bool hasMagneticNed(); + + /// \brief Magnetic NED data. + /// \return The magnetic NED data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f magneticNed(); + + /// \brief Indicates if magneticEcef has valid data. + /// \return true if magneticEcef has valid data; otherwise false. + bool hasMagneticEcef(); + + /// \brief Magnetic ECEF data. + /// \return The magnetic ECEF data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f magneticEcef(); + + + /// \brief Indicates if anyAcceleration has valid data. + /// \return true if anyAcceleration has valid data; otherwise false. + bool hasAnyAcceleration(); + + /// \brief Gets and converts the latest acceleration data regardless of the received + /// underlying type. + /// + /// \return The acceleration data. + /// \exception invalid_operation Thrown if there is no valid data. + math::vec3f anyAcceleration(); + + /// \brief Indicates if acceleration has valid data. + /// \return true if acceleration has valid data; otherwise false. + bool hasAcceleration(); + + /// \brief Acceleration data. + /// \return The acceleration data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f acceleration(); + + /// \brief Indicates if accelerationLinearBody has valid data. + /// \return true if accelerationLinearBody has valid data; otherwise false. + bool hasAccelerationLinearBody(); + + /// \brief Acceleration linear body data. + /// \return The acceleration linear body data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f accelerationLinearBody(); + + /// \brief Indicates if accelerationUncompensated has valid data. + /// \return true if accelerationUncompensated has valid data; otherwise false. + bool hasAccelerationUncompenated(); + + /// \brief Acceleration uncompensated data. + /// \return The acceleration uncompensated data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f accelerationUncompensated(); + + /// \brief Indicates if accelerationLinearNed has valid data. + /// \return true if accelerationLinearNed has valid data; otherwise false. + bool hasAccelerationLinearNed(); + + /// \brief Acceleration linear NED data. + /// \return The acceleration linear NED data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f accelerationLinearNed(); + + /// \brief Indicates if accelerationLinearEcef has valid data. + /// \return true if accelerationLinearEcef has valid data; otherwise false. + bool hasAccelerationLinearEcef(); + + /// \brief Acceleration linear ECEF data. + /// \return The acceleration linear ECEF data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f accelerationLinearEcef(); + + /// \brief Indicates if accelerationNed has valid data. + /// \return true if accelerationNed has valid data; otherwise false. + bool hasAccelerationNed(); + + /// \brief Acceleration NED data. + /// \return The acceleration NED data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f accelerationNed(); + + /// \brief Indicates if accelerationEcef has valid data. + /// \return true if accelerationEcef has valid data; otherwise false. + bool hasAccelerationEcef(); + + /// \brief Acceleration ECEF data. + /// \return The acceleration ECEF data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f accelerationEcef(); + + + /// \brief Indicates if anyAngularRate has valid data. + /// \return true if anyAngularRate has valid data; otherwise false. + bool hasAnyAngularRate(); + + /// \brief Gets and converts the latest angular rate data regardless of the received + /// underlying type. + /// + /// \return The angular rate data. + /// \exception invalid_operation Thrown if there is no valid data. + math::vec3f anyAngularRate(); + + /// \brief Indicates if angularRate has valid data. + /// \return true if angularRate has valid data; otherwise false. + bool hasAngularRate(); + + /// \brief Angular rate data. + /// \return The angular rate data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f angularRate(); + + /// \brief Indicates if angularRateUncompensated has valid data. + /// \return true if angularRateUncompensated has valid data; otherwise false. + bool hasAngularRateUncompensated(); + + /// \brief Angular rate uncompensated data. + /// \return The angular rate uncompensated data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f angularRateUncompensated(); + + + /// \brief Indicates if anyTemperature has valid data. + /// \return true if anyTemperature has valid data; otherwise false. + bool hasAnyTemperature(); + + /// \brief Gets and converts the latest temperature data regardless of the received + /// underlying type. + /// + /// \return The tempature data. + /// \exception invalid_operation Thrown if there is no valid data. + float anyTemperature(); + + /// \brief Indicates if temperature has valid data. + /// \return true if temperature has valid data; otherwise false. + bool hasTemperature(); + + /// \brief Temperature data. + /// \return The temperature data. + /// \exception invalid_operation Thrown if there is not any valid data. + float temperature(); + + + /// \brief Indicates if anyPressure has valid data. + /// \return true if anyPressure has valid data; otherwise false. + bool hasAnyPressure(); + + /// \brief Gets and converts the latest pressure data regardless of the received + /// underlying type. + /// + /// \return The pressure data. + /// \exception invalid_operation Thrown if there is no valid data. + float anyPressure(); + + /// \brief Indicates if pressure has valid data. + /// \return true if pressure has valid data; otherwise false. + bool hasPressure(); + + /// \brief Pressure data. + /// \return The pressure data. + /// \exception invalid_operation Thrown if there is not any valid data. + float pressure(); + + /// \brief Indicates if anyPosition has valid data. + /// \return true if anyPosition has valid data; otherwise false. + bool hasAnyPosition(); + + /// \brief Gets the latest position data regardless of the received + /// underlying type. + /// + /// \return The position data. + /// \exception invalid_operation Thrown if there is no valid data. + math::PositionD anyPosition(); + + /// \brief Indicates if positionGpsLla has valid data. + /// \return true if positionGpsLla has valid data; otherwise false. + bool hasPositionGpsLla(); + + /// \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 Indicates if positionGpsEcef has valid data. + /// \return true if positionGpsEcef has valid data; otherwise false. + bool hasPositionGpsEcef(); + + /// \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 Indicates if positionEstimatedLla has valid data. + /// \return true if positionEstimatedLla has valid data; otherwise false. + bool hasPositionEstimatedLla(); + + /// \brief Position Estimated LLA data. + /// \return The Position Estimated LLA data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3d positionEstimatedLla(); + + /// \brief Indicates if positionEstimatedEcef has valid data. + /// \return true if positionEstimatedEcef has valid data; otherwise false. + bool hasPositionEstimatedEcef(); + + /// \brief Position Estimated ECEF data. + /// \return The Position Estimated ECEF data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3d positionEstimatedEcef(); + + /// \brief Indicates if anyVelocity has valid data. + /// \return true if anyVelocity has valid data; otherwise false. + bool hasAnyVelocity(); + + /// \brief Gets the latest velocity data regardless of the received + /// underlying type. + /// + /// \return The velocity data. + /// \exception invalid_operation Thrown if there is no valid data. + math::vec3f anyVelocity(); + + /// \brief Indicates if velocityGpsNed has valid data. + /// \return true if velocityGpsNed has valid data; otherwise false. + bool hasVelocityGpsNed(); + + /// \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 Indicates if velocityGpsEcef has valid data. + /// \return true if velocityGpsEcef has valid data; otherwise false. + bool hasVelocityGpsEcef(); + + /// \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 Indicates if velocityEstimatedNed has valid data. + /// \return true if velocityEstimatedNed has valid data; otherwise false. + bool hasVelocityEstimatedNed(); + + /// \brief Velocity Estimated NED data. + /// \return The velocity estimated NED data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f velocityEstimatedNed(); + + /// \brief Indicates if velocityEstimatedEcef has valid data. + /// \return true if velocityEstimatedEcef has valid data; otherwise false. + bool hasVelocityEstimatedEcef(); + + /// \brief Velocity Estimated ECEF data. + /// \return The velocity estimated ECEF data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f velocityEstimatedEcef(); + + /// \brief Indicates if velocityEstimatedBody has valid data. + /// \return true if velocityEstimatedBody has valid data; otherwise false. + bool hasVelocityEstimatedBody(); + + /// \brief Velocity Estimated Body data. + /// \return The velocity estimated body data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f velocityEstimatedBody(); + + /// \brief Indicates if deltaTime has valid data. + /// \return true if deltaTime has valid data; otherwise false. + bool hasDeltaTime(); + + /// \brief Delta time data. + /// \return The delta time data. + /// \exception invalid_operation Thrown if there is not any valid data. + float deltaTime(); + + /// \brief Indicates if deltaTheta has valid data. + /// \return true if deltaTheta has valid data; otherwise false. + bool hasDeltaTheta(); + + /// \brief Delta theta data. + /// \return The delta theta data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f deltaTheta(); + + /// \brief Indicates if deltaVelocity has valid data. + /// \return true if deltaVelocity has valid data; otherwise false. + bool hasDeltaVelocity(); + + /// \brief Delta velocity data. + /// \return The delta velocity data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f deltaVelocity(); + + /// \brief Indicates if timeStartup has valid data. + /// \return true if timeStartup has valid data; otherwise false. + bool hasTimeStartup(); + + /// \brief Time startup data. + /// \return The time startup data. + /// \exception invalid_operation Thrown if there is not any valid data. + uint64_t timeStartup(); + + /// \brief Indicates if timeGps has valid data. + /// \return true if timeGps has valid data; otherwise false. + bool hasTimeGps(); + + /// \brief Time GPS data. + /// \return The time GPS data. + /// \exception invalid_operation Thrown if there is not any valid data. + uint64_t timeGps(); + + /// \brief Indicates if tow has valid data. + /// \return true if tow has valid data; otherwise false. + 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 week has valid data. + /// \return true if week has valid data; otherwise false. + bool hasWeek(); + + /// \brief Week data. + /// \return The week data. + /// \exception invalid_operation Thrown if there is not any valid data. + uint16_t week(); + + /// \brief Indicates if numSats has valid data. + /// \return true if numSats has valid data; otherwise false. + bool hasNumSats(); + + /// \brief NumSats data. + /// \return The numsats data. + /// \exception invalid_operation Thrown if there is not any valid data. + uint8_t numSats(); + + /// \brief Indicates if timeSyncIn has valid data. + /// \return true if timeSyncIn has valid data; otherwise false. + bool hasTimeSyncIn(); + + /// \brief TimeSyncIn data. + /// \return The TimeSyncIn data. + /// \exception invalid_operation Thrown if there is not any valid data. + uint64_t timeSyncIn(); + + /// \brief Indicates if vpeStatus has valid data. + /// \return true if vpeStatus has valid data; otherwise false. + bool hasVpeStatus(); + + /// \brief VpeStatus data. + /// \return The VpeStatus data. + /// \exception invalid_operation Thrown if there is not any valid data. + protocol::uart::VpeStatus vpeStatus(); + + /// \brief Indicates if insStatus has valid data. + /// \return true if insStatus has valid data; otherwise false. + bool hasInsStatus(); + + /// \brief InsStatus data. + /// \return The InsStatus data. + /// \exception invalid_operation Thrown if there is not any valid data. + protocol::uart::InsStatus insStatus(); + + /// \brief Indicates if syncInCnt has valid data. + /// \return true if syncInCnt has valid data; otherwise false. + bool hasSyncInCnt(); + + /// \brief SyncInCnt data. + /// \return The SyncInCnt data. + /// \exception invalid_operation Thrown if there is not any valid data. + uint32_t syncInCnt(); + + /// \brief Indicates if timeGpsPps has valid data. + /// \return true if timeGpsPps has valid data; otherwise false. + bool hasTimeGpsPps(); + + /// \brief TimeGpsPps data. + /// \return The TimeGpsPps data. + /// \exception invalid_operation Thrown if there is not any valid data. + uint64_t timeGpsPps(); + + /// \brief Indicates if gpsTow has valid data. + /// \return true if gpsTow has valid data; otherwise false. + bool hasGpsTow(); + + /// \brief GpsTow data. + /// \return The GpsTow data. + /// \exception invalid_operation Thrown if there is not any valid data. + uint64_t gpsTow(); + + /// \brief Indicates if timeUtc has valid data. + /// \return true if timeUtc has valid data; otherwise false. + bool hasTimeUtc(); + + /// \brief TimeUtc data. + /// \return The TimeUtc data. + /// \exception invalid_operation Thrown if there is not any valid data. + protocol::uart::TimeUtc timeUtc(); + + /// \brief Indicates if sensSat has valid data. + /// \return true if sensSat has valid data; otherwise false. + bool hasSensSat(); + + /// \brief SensSat data. + /// \return The SensSat data. + /// \exception invalid_operation Thrown if there is not any valid data. + protocol::uart::SensSat sensSat(); + + /// \brief Indicates if fix has valid data. + /// \return true if fix has valid data; otherwise false. + bool hasFix(); + + /// \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 Indicates if anyPositionUncertainty has valid data. + /// \return true if anyPositionUncertainty has valid data; otherwise false. + bool hasAnyPositionUncertainty(); + + /// \brief Gets the latest position uncertainty data regardless of the received + /// underlying type. + /// + /// \return The position uncertainty data. + /// \exception invalid_operation Thrown if there is no valid data. + math::vec3f anyPositionUncertainty(); + + /// \brief Indicates if positionUncertaintyGpsNed has valid data. + /// \return true if positionUncertaintyGpsNed has valid data; otherwise false. + bool hasPositionUncertaintyGpsNed(); + + /// \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 Indicates if positionUncertaintyGpsEcef has valid data. + /// \return true if positionUncertaintyGpsEcef has valid data; otherwise false. + bool hasPositionUncertaintyGpsEcef(); + + /// \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 Indicates if positionUncertaintyEstimated has valid data. + /// \return true if positionUncertaintyEstimated has valid data; otherwise false. + bool hasPositionUncertaintyEstimated(); + + /// \brief Estimated position uncertainty data. + /// \return The estimated position uncertainty data. + /// \exception invalid_operation Thrown if there is not any valid data. + float positionUncertaintyEstimated(); + + /// \brief Indicates if anyVelocityUncertainty has valid data. + /// \return true if anyVelocityUncertainty has valid data; otherwise false. + bool hasAnyVelocityUncertainty(); + + /// \brief Gets the latest velocity uncertainty data regardless of the received + /// underlying type. + /// + /// \return The velocity uncertainty data. + /// \exception invalid_operation Thrown if there is no valid data. + float anyVelocityUncertainty(); + + /// \brief Indicates if velocityUncertaintyGps has valid data. + /// \return true if velocityUncertaintyGps has valid data; otherwise false. + bool hasVelocityUncertaintyGps(); + + /// \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 Indicates if velocityUncertaintyEstimated has valid data. + /// \return true if velocityUncertaintyEstimated has valid data; otherwise false. + bool hasVelocityUncertaintyEstimated(); + + /// \brief Estimated velocity uncertainty data. + /// \return The estimated velocity uncertainty data. + /// \exception invalid_operation Thrown if there is not any valid data. + float velocityUncertaintyEstimated(); + + /// \brief Indicates if timeUncertainty has valid data. + /// \return true if timeUncertainty has valid data; otherwise false. + bool hasTimeUncertainty(); + + /// \brief Time uncertainty data. + /// \return The time uncertainty data. + /// \exception invalid_operation Thrown if there is not any valid data. + uint32_t timeUncertainty(); + + /// \brief Indicates if attitudeUncertainty has valid data. + /// \return true if attitudeUncertainty has valid data; otherwise false. + bool hasAttitudeUncertainty(); + + /// \brief Attitude uncertainty data. + /// \return The attitude uncertainty data. + /// \exception invalid_operation Thrown if there is not any valid data. + math::vec3f attitudeUncertainty(); + + /// \brief Indicates if courseOverGround has valid data. + /// \return true if courseOverGround havs valid data; otherwise false. + bool hasCourseOverGround(); + + /// \brief Computes the course over ground from any velocity data available. + /// + /// \return The computed course over ground. + /// \exception invalid_operation Thrown if there is no valid data. + float courseOverGround(); + + /// \brief Indicates if speedOverGround has valid data. + /// \return true if speedOverGround havs valid data; otherwise false. + bool hasSpeedOverGround(); + + /// \brief Computes the speed over ground from any velocity data available. + /// + /// \return The computed speed over ground. + /// \exception invalid_operation Thrown if there is no valid data. + float speedOverGround(); + + +private: + static void parseBinary(protocol::uart::Packet& p, std::vector& o); + static void parseAscii(protocol::uart::Packet& p, std::vector& o); + static void parseBinaryPacketCommonGroup(protocol::uart::Packet& p, protocol::uart::CommonGroup gf, std::vector& o); + static void parseBinaryPacketTimeGroup(protocol::uart::Packet& p, protocol::uart::TimeGroup gf, std::vector& o); + static void parseBinaryPacketImuGroup(protocol::uart::Packet& p, protocol::uart::ImuGroup gf, std::vector& o); + static void parseBinaryPacketGpsGroup(protocol::uart::Packet& p, protocol::uart::GpsGroup gf, std::vector& o); + static void parseBinaryPacketAttitudeGroup(protocol::uart::Packet& p, protocol::uart::AttitudeGroup gf, std::vector& o); + static void parseBinaryPacketInsGroup(protocol::uart::Packet& p, protocol::uart::InsGroup gf, std::vector& o); + +private: + struct Impl; + Impl* _i; + + template + static void setValues(T val, std::vector& o, void (Impl::* function)(T)) + { + for (std::vector::iterator i = o.begin(); i != o.end(); ++i) + (*((*i)->_i).*function)(val); + } +}; + + + +} +} + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/sensors/ezasyncdata.h b/ext/vnproglib-1.1.0.115/include/vn/sensors/ezasyncdata.h new file mode 100755 index 0000000..d460a59 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/sensors/ezasyncdata.h @@ -0,0 +1,75 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifndef _VNSENSORS_EZASYNCDATA_H_ +#define _VNSENSORS_EZASYNCDATA_H_ + +#include + +#include "vn/int.h" +#include "vn/util/nocopy.h" +#include "vn/sensors/sensors.h" +#include "vn/sensors/compositedata.h" +#include "vn/xplat/criticalsection.h" +#include "vn/xplat/event.h" +#include "vn/util/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 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 diff --git a/ext/vnproglib-1.1.0.115/include/vn/sensors/mock.h b/ext/vnproglib-1.1.0.115/include/vn/sensors/mock.h new file mode 100755 index 0000000..f9d78d1 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/sensors/mock.h @@ -0,0 +1,12 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifndef _VNSENSORS_MOCK_H_ +#define _VNSENSORS_MOCK_H_ + +namespace xplat { +namespace sensors { + +} +} + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/sensors/registers.h b/ext/vnproglib-1.1.0.115/include/vn/sensors/registers.h new file mode 100755 index 0000000..7a7b722 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/sensors/registers.h @@ -0,0 +1,1462 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifndef _VNSENSORS_REGISTERS_H_ +#define _VNSENSORS_REGISTERS_H_ + +#include "vn/int.h" +#include "vn/math/vector.h" +#include "vn/math/matrix.h" +#include "vn/protocol/uart/types.h" + +namespace vn { +namespace sensors { + +/// \defgroup registerStructures Register Structures +/// \brief These structures represent the various registers on a VecotorNav +/// sensor. +/// +/// \{ + +/// \brief Structure representing a Binary Output register. +/// +/// The field outputGroup available on the sensor's register is not necessary +/// in this structure since all read/writes operations will automatically +/// determine this from the settings for the individual groups within this +/// structure. +struct BinaryOutputRegister +{ + protocol::uart::AsyncMode asyncMode; ///< The asyncMode field. + uint16_t rateDivisor; ///< The rateDivisor field. + protocol::uart::CommonGroup commonField; ///< Group 1 (Common) + protocol::uart::TimeGroup timeField; ///< Group 2 (Time) + protocol::uart::ImuGroup imuField; ///< Group 3 (IMU) + protocol::uart::GpsGroup gpsField; ///< Group 4 (GPS) + protocol::uart::AttitudeGroup attitudeField; ///< Group 5 (Attitude) + protocol::uart::InsGroup insField; ///< Group 6 (INS) + + BinaryOutputRegister() : + asyncMode(protocol::uart::ASYNCMODE_NONE), + rateDivisor(0), + commonField(protocol::uart::COMMONGROUP_NONE), + timeField(protocol::uart::TIMEGROUP_NONE), + imuField(protocol::uart::IMUGROUP_NONE), + gpsField(protocol::uart::GPSGROUP_NONE), + attitudeField(protocol::uart::ATTITUDEGROUP_NONE), + insField(protocol::uart::INSGROUP_NONE) + { } + + /// \brief Creates an initializes a new BinaryOutputRegister structure. + /// + /// \param[in] asyncModeIn Value to initialize the asyncMode field with. + /// \param[in] rateDivisorIn Value to initialize the rateDivisor field with. + /// \param[in] commonFieldIn Value to initialize field 1 (Common) with. + /// \param[in] timeFieldIn Value to initialize field 2 (Time) with. + /// \param[in] imuFieldIn Value to initialize field 3 (IMU) with. + /// \param[in] gpsFieldIn Value to initialize field 4 (GPS) with. + /// \param[in] attitudeFieldIn Value to initialize field 5 (Attitude) with. + /// \param[in] insFieldIn Value to initialize field 6 (INS) with. + BinaryOutputRegister( + protocol::uart::AsyncMode asyncModeIn, + uint16_t rateDivisorIn, + protocol::uart::CommonGroup commonFieldIn, + protocol::uart::TimeGroup timeFieldIn, + protocol::uart::ImuGroup imuFieldIn, + protocol::uart::GpsGroup gpsFieldIn, + protocol::uart::AttitudeGroup attitudeFieldIn, + protocol::uart::InsGroup insFieldIn) : + asyncMode(asyncModeIn), + rateDivisor(rateDivisorIn), + commonField(commonFieldIn), + timeField(timeFieldIn), + imuField(imuFieldIn), + gpsField(gpsFieldIn), + attitudeField(attitudeFieldIn), + insField(insFieldIn) + { } + + BinaryOutputRegister( + uint16_t asyncModeIn, + uint16_t rateDivisorIn, + uint16_t commonFieldIn, + uint16_t timeFieldIn, + uint16_t imuFieldIn, + uint16_t gpsFieldIn, + uint16_t attitudeFieldIn, + uint16_t insFieldIn) : + asyncMode(static_cast(asyncModeIn)), + rateDivisor(rateDivisorIn), + commonField(static_cast(commonFieldIn)), + timeField(static_cast(timeFieldIn)), + imuField(static_cast(imuFieldIn)), + gpsField(static_cast(gpsFieldIn)), + attitudeField(static_cast(attitudeFieldIn)), + insField(static_cast(insFieldIn)) + { } +}; + +/// \brief Structure representing the Quaternion, Magnetic, Acceleration and Angular Rates register. +struct QuaternionMagneticAccelerationAndAngularRatesRegister +{ + math::vec4f quat; ///< The quat field. + math::vec3f mag; ///< The mag field. + math::vec3f accel; ///< The accel field. + math::vec3f gyro; ///< The gyro field. + + QuaternionMagneticAccelerationAndAngularRatesRegister() { } + + /// \brief Creates an initializes a new QuaternionMagneticAccelerationAndAngularRatesRegister structure. + /// + /// \param[in] quatIn Value to initialize the quat field with. + /// \param[in] magIn Value to initialize the mag field with. + /// \param[in] accelIn Value to initialize the accel field with. + /// \param[in] gyroIn Value to initialize the gyro field with. + QuaternionMagneticAccelerationAndAngularRatesRegister( + math::vec4f quatIn, + math::vec3f magIn, + math::vec3f accelIn, + math::vec3f gyroIn) : + quat(quatIn), + mag(magIn), + accel(accelIn), + gyro(gyroIn) + { } + +}; + +/// \brief Structure representing the Magnetic, Acceleration and Angular Rates register. +struct MagneticAccelerationAndAngularRatesRegister +{ + math::vec3f mag; ///< The mag field. + math::vec3f accel; ///< The accel field. + math::vec3f gyro; ///< The gyro field. + + MagneticAccelerationAndAngularRatesRegister() { } + + /// \brief Creates an initializes a new MagneticAccelerationAndAngularRatesRegister structure. + /// + /// \param[in] magIn Value to initialize the mag field with. + /// \param[in] accelIn Value to initialize the accel field with. + /// \param[in] gyroIn Value to initialize the gyro field with. + MagneticAccelerationAndAngularRatesRegister( + math::vec3f magIn, + math::vec3f accelIn, + math::vec3f gyroIn) : + mag(magIn), + accel(accelIn), + gyro(gyroIn) + { } + +}; + +/// \brief Structure representing the Magnetic and Gravity Reference Vectors register. +struct MagneticAndGravityReferenceVectorsRegister +{ + math::vec3f magRef; ///< The magRef field. + math::vec3f accRef; ///< The accRef field. + + MagneticAndGravityReferenceVectorsRegister() { } + + /// \brief Creates an initializes a new MagneticAndGravityReferenceVectorsRegister structure. + /// + /// \param[in] magRefIn Value to initialize the magRef field with. + /// \param[in] accRefIn Value to initialize the accRef field with. + MagneticAndGravityReferenceVectorsRegister( + math::vec3f magRefIn, + math::vec3f accRefIn) : + magRef(magRefIn), + accRef(accRefIn) + { } + +}; + +/// \brief Structure representing the Filter Measurements Variance Parameters register. +struct FilterMeasurementsVarianceParametersRegister +{ + float angularWalkVariance; ///< The angularWalkVariance field. + math::vec3f angularRateVariance; ///< The angularRateVariance field. + math::vec3f magneticVariance; ///< The magneticVariance field. + math::vec3f accelerationVariance; ///< The accelerationVariance field. + + FilterMeasurementsVarianceParametersRegister() { } + + /// \brief Creates an initializes a new FilterMeasurementsVarianceParametersRegister structure. + /// + /// \param[in] angularWalkVarianceIn Value to initialize the angularWalkVariance field with. + /// \param[in] angularRateVarianceIn Value to initialize the angularRateVariance field with. + /// \param[in] magneticVarianceIn Value to initialize the magneticVariance field with. + /// \param[in] accelerationVarianceIn Value to initialize the accelerationVariance field with. + FilterMeasurementsVarianceParametersRegister( + float angularWalkVarianceIn, + math::vec3f angularRateVarianceIn, + math::vec3f magneticVarianceIn, + math::vec3f accelerationVarianceIn) : + angularWalkVariance(angularWalkVarianceIn), + angularRateVariance(angularRateVarianceIn), + magneticVariance(magneticVarianceIn), + accelerationVariance(accelerationVarianceIn) + { } + +}; + +/// \brief Structure representing the Magnetometer Compensation register. +struct MagnetometerCompensationRegister +{ + math::mat3f c; ///< The c field. + math::vec3f b; ///< The b field. + + MagnetometerCompensationRegister() { } + + /// \brief Creates an initializes a new MagnetometerCompensationRegister structure. + /// + /// \param[in] cIn Value to initialize the c field with. + /// \param[in] bIn Value to initialize the b field with. + MagnetometerCompensationRegister( + math::mat3f cIn, + math::vec3f bIn) : + c(cIn), + b(bIn) + { } + +}; + +/// \brief Structure representing the Filter Active Tuning Parameters register. +struct FilterActiveTuningParametersRegister +{ + float magneticDisturbanceGain; ///< The magneticDisturbanceGain field. + float accelerationDisturbanceGain; ///< The accelerationDisturbanceGain field. + float magneticDisturbanceMemory; ///< The magneticDisturbanceMemory field. + float accelerationDisturbanceMemory; ///< The accelerationDisturbanceMemory field. + + FilterActiveTuningParametersRegister() { } + + /// \brief Creates an initializes a new FilterActiveTuningParametersRegister structure. + /// + /// \param[in] magneticDisturbanceGainIn Value to initialize the magneticDisturbanceGain field with. + /// \param[in] accelerationDisturbanceGainIn Value to initialize the accelerationDisturbanceGain field with. + /// \param[in] magneticDisturbanceMemoryIn Value to initialize the magneticDisturbanceMemory field with. + /// \param[in] accelerationDisturbanceMemoryIn Value to initialize the accelerationDisturbanceMemory field with. + FilterActiveTuningParametersRegister( + float magneticDisturbanceGainIn, + float accelerationDisturbanceGainIn, + float magneticDisturbanceMemoryIn, + float accelerationDisturbanceMemoryIn) : + magneticDisturbanceGain(magneticDisturbanceGainIn), + accelerationDisturbanceGain(accelerationDisturbanceGainIn), + magneticDisturbanceMemory(magneticDisturbanceMemoryIn), + accelerationDisturbanceMemory(accelerationDisturbanceMemoryIn) + { } + +}; + +/// \brief Structure representing the Acceleration Compensation register. +struct AccelerationCompensationRegister +{ + math::mat3f c; ///< The c field. + math::vec3f b; ///< The b field. + + AccelerationCompensationRegister() { } + + /// \brief Creates an initializes a new AccelerationCompensationRegister structure. + /// + /// \param[in] cIn Value to initialize the c field with. + /// \param[in] bIn Value to initialize the b field with. + AccelerationCompensationRegister( + math::mat3f cIn, + math::vec3f bIn) : + c(cIn), + b(bIn) + { } + +}; + +/// \brief Structure representing the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register. +struct YawPitchRollMagneticAccelerationAndAngularRatesRegister +{ + math::vec3f yawPitchRoll; ///< The yawPitchRoll field. + math::vec3f mag; ///< The mag field. + math::vec3f accel; ///< The accel field. + math::vec3f gyro; ///< The gyro field. + + YawPitchRollMagneticAccelerationAndAngularRatesRegister() { } + + /// \brief Creates an initializes a new YawPitchRollMagneticAccelerationAndAngularRatesRegister structure. + /// + /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. + /// \param[in] magIn Value to initialize the mag field with. + /// \param[in] accelIn Value to initialize the accel field with. + /// \param[in] gyroIn Value to initialize the gyro field with. + YawPitchRollMagneticAccelerationAndAngularRatesRegister( + math::vec3f yawPitchRollIn, + math::vec3f magIn, + math::vec3f accelIn, + math::vec3f gyroIn) : + yawPitchRoll(yawPitchRollIn), + mag(magIn), + accel(accelIn), + gyro(gyroIn) + { } + +}; + +/// \brief Structure representing the Communication Protocol Control register. +struct CommunicationProtocolControlRegister +{ + protocol::uart::CountMode serialCount; ///< The serialCount field. + protocol::uart::StatusMode serialStatus; ///< The serialStatus field. + protocol::uart::CountMode spiCount; ///< The spiCount field. + protocol::uart::StatusMode spiStatus; ///< The spiStatus field. + protocol::uart::ChecksumMode serialChecksum; ///< The serialChecksum field. + protocol::uart::ChecksumMode spiChecksum; ///< The spiChecksum field. + protocol::uart::ErrorMode errorMode; ///< The errorMode field. + + CommunicationProtocolControlRegister() { } + + /// \brief Creates an initializes a new CommunicationProtocolControlRegister structure. + /// + /// \param[in] serialCountIn Value to initialize the serialCount field with. + /// \param[in] serialStatusIn Value to initialize the serialStatus field with. + /// \param[in] spiCountIn Value to initialize the spiCount field with. + /// \param[in] spiStatusIn Value to initialize the spiStatus field with. + /// \param[in] serialChecksumIn Value to initialize the serialChecksum field with. + /// \param[in] spiChecksumIn Value to initialize the spiChecksum field with. + /// \param[in] errorModeIn Value to initialize the errorMode field with. + CommunicationProtocolControlRegister( + protocol::uart::CountMode serialCountIn, + protocol::uart::StatusMode serialStatusIn, + protocol::uart::CountMode spiCountIn, + protocol::uart::StatusMode spiStatusIn, + protocol::uart::ChecksumMode serialChecksumIn, + protocol::uart::ChecksumMode spiChecksumIn, + protocol::uart::ErrorMode errorModeIn) : + serialCount(serialCountIn), + serialStatus(serialStatusIn), + spiCount(spiCountIn), + spiStatus(spiStatusIn), + serialChecksum(serialChecksumIn), + spiChecksum(spiChecksumIn), + errorMode(errorModeIn) + { } + +}; + +/// \brief Structure representing the Synchronization Control register. +struct SynchronizationControlRegister +{ + protocol::uart::SyncInMode syncInMode; ///< The syncInMode field. + protocol::uart::SyncInEdge syncInEdge; ///< The syncInEdge field. + uint16_t syncInSkipFactor; ///< The syncInSkipFactor field. + protocol::uart::SyncOutMode syncOutMode; ///< The syncOutMode field. + protocol::uart::SyncOutPolarity syncOutPolarity; ///< The syncOutPolarity field. + uint16_t syncOutSkipFactor; ///< The syncOutSkipFactor field. + uint32_t syncOutPulseWidth; ///< The syncOutPulseWidth field. + + SynchronizationControlRegister() { } + + /// \brief Creates an initializes a new SynchronizationControlRegister structure. + /// + /// \param[in] syncInModeIn Value to initialize the syncInMode field with. + /// \param[in] syncInEdgeIn Value to initialize the syncInEdge field with. + /// \param[in] syncInSkipFactorIn Value to initialize the syncInSkipFactor field with. + /// \param[in] syncOutModeIn Value to initialize the syncOutMode field with. + /// \param[in] syncOutPolarityIn Value to initialize the syncOutPolarity field with. + /// \param[in] syncOutSkipFactorIn Value to initialize the syncOutSkipFactor field with. + /// \param[in] syncOutPulseWidthIn Value to initialize the syncOutPulseWidth field with. + SynchronizationControlRegister( + protocol::uart::SyncInMode syncInModeIn, + protocol::uart::SyncInEdge syncInEdgeIn, + uint16_t syncInSkipFactorIn, + protocol::uart::SyncOutMode syncOutModeIn, + protocol::uart::SyncOutPolarity syncOutPolarityIn, + uint16_t syncOutSkipFactorIn, + uint32_t syncOutPulseWidthIn) : + syncInMode(syncInModeIn), + syncInEdge(syncInEdgeIn), + syncInSkipFactor(syncInSkipFactorIn), + syncOutMode(syncOutModeIn), + syncOutPolarity(syncOutPolarityIn), + syncOutSkipFactor(syncOutSkipFactorIn), + syncOutPulseWidth(syncOutPulseWidthIn) + { } + +}; + +/// \brief Structure representing the Synchronization Status register. +struct SynchronizationStatusRegister +{ + uint32_t syncInCount; ///< The syncInCount field. + uint32_t syncInTime; ///< The syncInTime field. + uint32_t syncOutCount; ///< The syncOutCount field. + + SynchronizationStatusRegister() { } + + /// \brief Creates an initializes a new SynchronizationStatusRegister structure. + /// + /// \param[in] syncInCountIn Value to initialize the syncInCount field with. + /// \param[in] syncInTimeIn Value to initialize the syncInTime field with. + /// \param[in] syncOutCountIn Value to initialize the syncOutCount field with. + SynchronizationStatusRegister( + uint32_t syncInCountIn, + uint32_t syncInTimeIn, + uint32_t syncOutCountIn) : + syncInCount(syncInCountIn), + syncInTime(syncInTimeIn), + syncOutCount(syncOutCountIn) + { } + +}; + +/// \brief Structure representing the Filter Basic Control register. +struct FilterBasicControlRegister +{ + protocol::uart::MagneticMode magMode; ///< The magMode field. + protocol::uart::ExternalSensorMode extMagMode; ///< The extMagMode field. + protocol::uart::ExternalSensorMode extAccMode; ///< The extAccMode field. + protocol::uart::ExternalSensorMode extGyroMode; ///< The extGyroMode field. + math::vec3f gyroLimit; ///< The gyroLimit field. + + FilterBasicControlRegister() { } + + /// \brief Creates an initializes a new FilterBasicControlRegister structure. + /// + /// \param[in] magModeIn Value to initialize the magMode field with. + /// \param[in] extMagModeIn Value to initialize the extMagMode field with. + /// \param[in] extAccModeIn Value to initialize the extAccMode field with. + /// \param[in] extGyroModeIn Value to initialize the extGyroMode field with. + /// \param[in] gyroLimitIn Value to initialize the gyroLimit field with. + FilterBasicControlRegister( + protocol::uart::MagneticMode magModeIn, + protocol::uart::ExternalSensorMode extMagModeIn, + protocol::uart::ExternalSensorMode extAccModeIn, + protocol::uart::ExternalSensorMode extGyroModeIn, + math::vec3f gyroLimitIn) : + magMode(magModeIn), + extMagMode(extMagModeIn), + extAccMode(extAccModeIn), + extGyroMode(extGyroModeIn), + gyroLimit(gyroLimitIn) + { } + +}; + +/// \brief Structure representing the VPE Basic Control register. +struct VpeBasicControlRegister +{ + protocol::uart::VpeEnable enable; ///< The enable field. + protocol::uart::HeadingMode headingMode; ///< The headingMode field. + protocol::uart::VpeMode filteringMode; ///< The filteringMode field. + protocol::uart::VpeMode tuningMode; ///< The tuningMode field. + + VpeBasicControlRegister() { } + + /// \brief Creates an initializes a new VpeBasicControlRegister structure. + /// + /// \param[in] enableIn Value to initialize the enable field with. + /// \param[in] headingModeIn Value to initialize the headingMode field with. + /// \param[in] filteringModeIn Value to initialize the filteringMode field with. + /// \param[in] tuningModeIn Value to initialize the tuningMode field with. + VpeBasicControlRegister( + protocol::uart::VpeEnable enableIn, + protocol::uart::HeadingMode headingModeIn, + protocol::uart::VpeMode filteringModeIn, + protocol::uart::VpeMode tuningModeIn) : + enable(enableIn), + headingMode(headingModeIn), + filteringMode(filteringModeIn), + tuningMode(tuningModeIn) + { } + +}; + +/// \brief Structure representing the VPE Magnetometer Basic Tuning register. +struct VpeMagnetometerBasicTuningRegister +{ + math::vec3f baseTuning; ///< The baseTuning field. + math::vec3f adaptiveTuning; ///< The adaptiveTuning field. + math::vec3f adaptiveFiltering; ///< The adaptiveFiltering field. + + VpeMagnetometerBasicTuningRegister() { } + + /// \brief Creates an initializes a new VpeMagnetometerBasicTuningRegister structure. + /// + /// \param[in] baseTuningIn Value to initialize the baseTuning field with. + /// \param[in] adaptiveTuningIn Value to initialize the adaptiveTuning field with. + /// \param[in] adaptiveFilteringIn Value to initialize the adaptiveFiltering field with. + VpeMagnetometerBasicTuningRegister( + math::vec3f baseTuningIn, + math::vec3f adaptiveTuningIn, + math::vec3f adaptiveFilteringIn) : + baseTuning(baseTuningIn), + adaptiveTuning(adaptiveTuningIn), + adaptiveFiltering(adaptiveFilteringIn) + { } + +}; + +/// \brief Structure representing the VPE Magnetometer Advanced Tuning register. +struct VpeMagnetometerAdvancedTuningRegister +{ + math::vec3f minFiltering; ///< The minFiltering field. + math::vec3f maxFiltering; ///< The maxFiltering field. + float maxAdaptRate; ///< The maxAdaptRate field. + float disturbanceWindow; ///< The disturbanceWindow field. + float maxTuning; ///< The maxTuning field. + + VpeMagnetometerAdvancedTuningRegister() { } + + /// \brief Creates an initializes a new VpeMagnetometerAdvancedTuningRegister structure. + /// + /// \param[in] minFilteringIn Value to initialize the minFiltering field with. + /// \param[in] maxFilteringIn Value to initialize the maxFiltering field with. + /// \param[in] maxAdaptRateIn Value to initialize the maxAdaptRate field with. + /// \param[in] disturbanceWindowIn Value to initialize the disturbanceWindow field with. + /// \param[in] maxTuningIn Value to initialize the maxTuning field with. + VpeMagnetometerAdvancedTuningRegister( + math::vec3f minFilteringIn, + math::vec3f maxFilteringIn, + float maxAdaptRateIn, + float disturbanceWindowIn, + float maxTuningIn) : + minFiltering(minFilteringIn), + maxFiltering(maxFilteringIn), + maxAdaptRate(maxAdaptRateIn), + disturbanceWindow(disturbanceWindowIn), + maxTuning(maxTuningIn) + { } + +}; + +/// \brief Structure representing the VPE Accelerometer Basic Tuning register. +struct VpeAccelerometerBasicTuningRegister +{ + math::vec3f baseTuning; ///< The baseTuning field. + math::vec3f adaptiveTuning; ///< The adaptiveTuning field. + math::vec3f adaptiveFiltering; ///< The adaptiveFiltering field. + + VpeAccelerometerBasicTuningRegister() { } + + /// \brief Creates an initializes a new VpeAccelerometerBasicTuningRegister structure. + /// + /// \param[in] baseTuningIn Value to initialize the baseTuning field with. + /// \param[in] adaptiveTuningIn Value to initialize the adaptiveTuning field with. + /// \param[in] adaptiveFilteringIn Value to initialize the adaptiveFiltering field with. + VpeAccelerometerBasicTuningRegister( + math::vec3f baseTuningIn, + math::vec3f adaptiveTuningIn, + math::vec3f adaptiveFilteringIn) : + baseTuning(baseTuningIn), + adaptiveTuning(adaptiveTuningIn), + adaptiveFiltering(adaptiveFilteringIn) + { } + +}; + +/// \brief Structure representing the VPE Accelerometer Advanced Tuning register. +struct VpeAccelerometerAdvancedTuningRegister +{ + math::vec3f minFiltering; ///< The minFiltering field. + math::vec3f maxFiltering; ///< The maxFiltering field. + float maxAdaptRate; ///< The maxAdaptRate field. + float disturbanceWindow; ///< The disturbanceWindow field. + float maxTuning; ///< The maxTuning field. + + VpeAccelerometerAdvancedTuningRegister() { } + + /// \brief Creates an initializes a new VpeAccelerometerAdvancedTuningRegister structure. + /// + /// \param[in] minFilteringIn Value to initialize the minFiltering field with. + /// \param[in] maxFilteringIn Value to initialize the maxFiltering field with. + /// \param[in] maxAdaptRateIn Value to initialize the maxAdaptRate field with. + /// \param[in] disturbanceWindowIn Value to initialize the disturbanceWindow field with. + /// \param[in] maxTuningIn Value to initialize the maxTuning field with. + VpeAccelerometerAdvancedTuningRegister( + math::vec3f minFilteringIn, + math::vec3f maxFilteringIn, + float maxAdaptRateIn, + float disturbanceWindowIn, + float maxTuningIn) : + minFiltering(minFilteringIn), + maxFiltering(maxFilteringIn), + maxAdaptRate(maxAdaptRateIn), + disturbanceWindow(disturbanceWindowIn), + maxTuning(maxTuningIn) + { } + +}; + +/// \brief Structure representing the VPE Gryo Basic Tuning register. +struct VpeGryoBasicTuningRegister +{ + math::vec3f angularWalkVariance; ///< The angularWalkVariance field. + math::vec3f baseTuning; ///< The baseTuning field. + math::vec3f adaptiveTuning; ///< The adaptiveTuning field. + + VpeGryoBasicTuningRegister() { } + + /// \brief Creates an initializes a new VpeGryoBasicTuningRegister structure. + /// + /// \param[in] angularWalkVarianceIn Value to initialize the angularWalkVariance field with. + /// \param[in] baseTuningIn Value to initialize the baseTuning field with. + /// \param[in] adaptiveTuningIn Value to initialize the adaptiveTuning field with. + VpeGryoBasicTuningRegister( + math::vec3f angularWalkVarianceIn, + math::vec3f baseTuningIn, + math::vec3f adaptiveTuningIn) : + angularWalkVariance(angularWalkVarianceIn), + baseTuning(baseTuningIn), + adaptiveTuning(adaptiveTuningIn) + { } + +}; + +/// \brief Structure representing the Magnetometer Calibration Control register. +struct MagnetometerCalibrationControlRegister +{ + protocol::uart::HsiMode hsiMode; ///< The hsiMode field. + protocol::uart::HsiOutput hsiOutput; ///< The hsiOutput field. + uint8_t convergeRate; ///< The convergeRate field. + + MagnetometerCalibrationControlRegister() { } + + /// \brief Creates an initializes a new MagnetometerCalibrationControlRegister structure. + /// + /// \param[in] hsiModeIn Value to initialize the hsiMode field with. + /// \param[in] hsiOutputIn Value to initialize the hsiOutput field with. + /// \param[in] convergeRateIn Value to initialize the convergeRate field with. + MagnetometerCalibrationControlRegister( + protocol::uart::HsiMode hsiModeIn, + protocol::uart::HsiOutput hsiOutputIn, + uint8_t convergeRateIn) : + hsiMode(hsiModeIn), + hsiOutput(hsiOutputIn), + convergeRate(convergeRateIn) + { } + +}; + +/// \brief Structure representing the Calculated Magnetometer Calibration register. +struct CalculatedMagnetometerCalibrationRegister +{ + math::mat3f c; ///< The c field. + math::vec3f b; ///< The b field. + + CalculatedMagnetometerCalibrationRegister() { } + + /// \brief Creates an initializes a new CalculatedMagnetometerCalibrationRegister structure. + /// + /// \param[in] cIn Value to initialize the c field with. + /// \param[in] bIn Value to initialize the b field with. + CalculatedMagnetometerCalibrationRegister( + math::mat3f cIn, + math::vec3f bIn) : + c(cIn), + b(bIn) + { } + +}; + +/// \brief Structure representing the Velocity Compensation Control register. +struct VelocityCompensationControlRegister +{ + protocol::uart::VelocityCompensationMode mode; ///< The mode field. + float velocityTuning; ///< The velocityTuning field. + float rateTuning; ///< The rateTuning field. + + VelocityCompensationControlRegister() { } + + /// \brief Creates an initializes a new VelocityCompensationControlRegister structure. + /// + /// \param[in] modeIn Value to initialize the mode field with. + /// \param[in] velocityTuningIn Value to initialize the velocityTuning field with. + /// \param[in] rateTuningIn Value to initialize the rateTuning field with. + VelocityCompensationControlRegister( + protocol::uart::VelocityCompensationMode modeIn, + float velocityTuningIn, + float rateTuningIn) : + mode(modeIn), + velocityTuning(velocityTuningIn), + rateTuning(rateTuningIn) + { } + +}; + +/// \brief Structure representing the Velocity Compensation Status register. +struct VelocityCompensationStatusRegister +{ + float x; ///< The x field. + float xDot; ///< The xDot field. + math::vec3f accelOffset; ///< The accelOffset field. + math::vec3f omega; ///< The omega field. + + VelocityCompensationStatusRegister() { } + + /// \brief Creates an initializes a new VelocityCompensationStatusRegister structure. + /// + /// \param[in] xIn Value to initialize the x field with. + /// \param[in] xDotIn Value to initialize the xDot field with. + /// \param[in] accelOffsetIn Value to initialize the accelOffset field with. + /// \param[in] omegaIn Value to initialize the omega field with. + VelocityCompensationStatusRegister( + float xIn, + float xDotIn, + math::vec3f accelOffsetIn, + math::vec3f omegaIn) : + x(xIn), + xDot(xDotIn), + accelOffset(accelOffsetIn), + omega(omegaIn) + { } + +}; + +/// \brief Structure representing the IMU Measurements register. +struct ImuMeasurementsRegister +{ + math::vec3f mag; ///< The mag field. + math::vec3f accel; ///< The accel field. + math::vec3f gyro; ///< The gyro field. + float temp; ///< The temp field. + float pressure; ///< The pressure field. + + ImuMeasurementsRegister() { } + + /// \brief Creates an initializes a new ImuMeasurementsRegister structure. + /// + /// \param[in] magIn Value to initialize the mag field with. + /// \param[in] accelIn Value to initialize the accel field with. + /// \param[in] gyroIn Value to initialize the gyro field with. + /// \param[in] tempIn Value to initialize the temp field with. + /// \param[in] pressureIn Value to initialize the pressure field with. + ImuMeasurementsRegister( + math::vec3f magIn, + math::vec3f accelIn, + math::vec3f gyroIn, + float tempIn, + float pressureIn) : + mag(magIn), + accel(accelIn), + gyro(gyroIn), + temp(tempIn), + pressure(pressureIn) + { } + +}; + +/// \brief Structure representing the GPS Configuration register. +struct GpsConfigurationRegister +{ + protocol::uart::GpsMode mode; ///< The mode field. + protocol::uart::PpsSource ppsSource; ///< The ppsSource field. + + GpsConfigurationRegister() { } + + /// \brief Creates an initializes a new GpsConfigurationRegister structure. + /// + /// \param[in] modeIn Value to initialize the mode field with. + /// \param[in] ppsSourceIn Value to initialize the ppsSource field with. + GpsConfigurationRegister( + protocol::uart::GpsMode modeIn, + protocol::uart::PpsSource ppsSourceIn) : + mode(modeIn), + ppsSource(ppsSourceIn) + { } + +}; + +/// \brief Structure representing the GPS Solution - LLA register. +struct GpsSolutionLlaRegister +{ + double time; ///< The time field. + uint16_t week; ///< The week field. + protocol::uart::GpsFix gpsFix; ///< The gpsFix field. + uint8_t numSats; ///< The numSats field. + math::vec3d lla; ///< The lla field. + math::vec3f nedVel; ///< The nedVel field. + math::vec3f nedAcc; ///< The nedAcc field. + float speedAcc; ///< The speedAcc field. + float timeAcc; ///< The timeAcc field. + + GpsSolutionLlaRegister() { } + + /// \brief Creates an initializes a new GpsSolutionLlaRegister structure. + /// + /// \param[in] timeIn Value to initialize the time field with. + /// \param[in] weekIn Value to initialize the week field with. + /// \param[in] gpsFixIn Value to initialize the gpsFix field with. + /// \param[in] numSatsIn Value to initialize the numSats field with. + /// \param[in] llaIn Value to initialize the lla field with. + /// \param[in] nedVelIn Value to initialize the nedVel field with. + /// \param[in] nedAccIn Value to initialize the nedAcc field with. + /// \param[in] speedAccIn Value to initialize the speedAcc field with. + /// \param[in] timeAccIn Value to initialize the timeAcc field with. + GpsSolutionLlaRegister( + double timeIn, + uint16_t weekIn, + protocol::uart::GpsFix gpsFixIn, + uint8_t numSatsIn, + math::vec3d llaIn, + math::vec3f nedVelIn, + math::vec3f nedAccIn, + float speedAccIn, + float timeAccIn) : + time(timeIn), + week(weekIn), + gpsFix(gpsFixIn), + numSats(numSatsIn), + lla(llaIn), + nedVel(nedVelIn), + nedAcc(nedAccIn), + speedAcc(speedAccIn), + timeAcc(timeAccIn) + { } + +}; + +/// \brief Structure representing the GPS Solution - ECEF register. +struct GpsSolutionEcefRegister +{ + double tow; ///< The tow field. + uint16_t week; ///< The week field. + protocol::uart::GpsFix gpsFix; ///< The gpsFix field. + uint8_t numSats; ///< The numSats field. + math::vec3d position; ///< The position field. + math::vec3f velocity; ///< The velocity field. + math::vec3f posAcc; ///< The posAcc field. + float speedAcc; ///< The speedAcc field. + float timeAcc; ///< The timeAcc field. + + GpsSolutionEcefRegister() { } + + /// \brief Creates an initializes a new GpsSolutionEcefRegister structure. + /// + /// \param[in] towIn Value to initialize the tow field with. + /// \param[in] weekIn Value to initialize the week field with. + /// \param[in] gpsFixIn Value to initialize the gpsFix field with. + /// \param[in] numSatsIn Value to initialize the numSats field with. + /// \param[in] positionIn Value to initialize the position field with. + /// \param[in] velocityIn Value to initialize the velocity field with. + /// \param[in] posAccIn Value to initialize the posAcc field with. + /// \param[in] speedAccIn Value to initialize the speedAcc field with. + /// \param[in] timeAccIn Value to initialize the timeAcc field with. + GpsSolutionEcefRegister( + double towIn, + uint16_t weekIn, + protocol::uart::GpsFix gpsFixIn, + uint8_t numSatsIn, + math::vec3d positionIn, + math::vec3f velocityIn, + math::vec3f posAccIn, + float speedAccIn, + float timeAccIn) : + tow(towIn), + week(weekIn), + gpsFix(gpsFixIn), + numSats(numSatsIn), + position(positionIn), + velocity(velocityIn), + posAcc(posAccIn), + speedAcc(speedAccIn), + timeAcc(timeAccIn) + { } + +}; + +/// \brief Structure representing the INS Solution - LLA register. +struct InsSolutionLlaRegister +{ + double time; ///< The time field. + uint16_t week; ///< The week field. + uint16_t status; ///< The status field. + math::vec3f yawPitchRoll; ///< The yawPitchRoll field. + math::vec3d position; ///< The position field. + math::vec3f nedVel; ///< The nedVel field. + float attUncertainty; ///< The attUncertainty field. + float posUncertainty; ///< The posUncertainty field. + float velUncertainty; ///< The velUncertainty field. + + InsSolutionLlaRegister() { } + + /// \brief Creates an initializes a new InsSolutionLlaRegister structure. + /// + /// \param[in] timeIn Value to initialize the time field with. + /// \param[in] weekIn Value to initialize the week field with. + /// \param[in] statusIn Value to initialize the status field with. + /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. + /// \param[in] positionIn Value to initialize the position field with. + /// \param[in] nedVelIn Value to initialize the nedVel field with. + /// \param[in] attUncertaintyIn Value to initialize the attUncertainty field with. + /// \param[in] posUncertaintyIn Value to initialize the posUncertainty field with. + /// \param[in] velUncertaintyIn Value to initialize the velUncertainty field with. + InsSolutionLlaRegister( + double timeIn, + uint16_t weekIn, + uint16_t statusIn, + math::vec3f yawPitchRollIn, + math::vec3d positionIn, + math::vec3f nedVelIn, + float attUncertaintyIn, + float posUncertaintyIn, + float velUncertaintyIn) : + time(timeIn), + week(weekIn), + status(statusIn), + yawPitchRoll(yawPitchRollIn), + position(positionIn), + nedVel(nedVelIn), + attUncertainty(attUncertaintyIn), + posUncertainty(posUncertaintyIn), + velUncertainty(velUncertaintyIn) + { } + +}; + +/// \brief Structure representing the INS Solution - ECEF register. +struct InsSolutionEcefRegister +{ + double time; ///< The time field. + uint16_t week; ///< The week field. + uint16_t status; ///< The status field. + math::vec3f yawPitchRoll; ///< The yawPitchRoll field. + math::vec3d position; ///< The position field. + math::vec3f velocity; ///< The velocity field. + float attUncertainty; ///< The attUncertainty field. + float posUncertainty; ///< The posUncertainty field. + float velUncertainty; ///< The velUncertainty field. + + InsSolutionEcefRegister() { } + + /// \brief Creates an initializes a new InsSolutionEcefRegister structure. + /// + /// \param[in] timeIn Value to initialize the time field with. + /// \param[in] weekIn Value to initialize the week field with. + /// \param[in] statusIn Value to initialize the status field with. + /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. + /// \param[in] positionIn Value to initialize the position field with. + /// \param[in] velocityIn Value to initialize the velocity field with. + /// \param[in] attUncertaintyIn Value to initialize the attUncertainty field with. + /// \param[in] posUncertaintyIn Value to initialize the posUncertainty field with. + /// \param[in] velUncertaintyIn Value to initialize the velUncertainty field with. + InsSolutionEcefRegister( + double timeIn, + uint16_t weekIn, + uint16_t statusIn, + math::vec3f yawPitchRollIn, + math::vec3d positionIn, + math::vec3f velocityIn, + float attUncertaintyIn, + float posUncertaintyIn, + float velUncertaintyIn) : + time(timeIn), + week(weekIn), + status(statusIn), + yawPitchRoll(yawPitchRollIn), + position(positionIn), + velocity(velocityIn), + attUncertainty(attUncertaintyIn), + posUncertainty(posUncertaintyIn), + velUncertainty(velUncertaintyIn) + { } + +}; + +/// \brief Structure representing the INS Basic Configuration register for a VN-200 sensor. +struct InsBasicConfigurationRegisterVn200 +{ + protocol::uart::Scenario scenario; ///< The scenario field. + bool ahrsAiding; ///< The ahrsAiding field. + + InsBasicConfigurationRegisterVn200() { } + + /// \brief Creates an initializes a new InsBasicConfigurationRegisterVn200 structure. + /// + /// \param[in] scenarioIn Value to initialize the scenario field with. + /// \param[in] ahrsAidingIn Value to initialize the ahrsAiding field with. + InsBasicConfigurationRegisterVn200( + protocol::uart::Scenario scenarioIn, + bool ahrsAidingIn) : + scenario(scenarioIn), + ahrsAiding(ahrsAidingIn) + { } + +}; + +/// \brief Structure representing the INS Basic Configuration register for a VN-300 sensor. +struct InsBasicConfigurationRegisterVn300 +{ + protocol::uart::Scenario scenario; ///< The scenario field. + bool ahrsAiding; ///< The ahrsAiding field. + bool estBaseline; ///< The estBaseline field. + + InsBasicConfigurationRegisterVn300() { } + + /// \brief Creates an initializes a new InsBasicConfigurationRegisterVn300 structure. + /// + /// \param[in] scenarioIn Value to initialize the scenario field with. + /// \param[in] ahrsAidingIn Value to initialize the ahrsAiding field with. + /// \param[in] estBaselineIn Value to initialize the estBaseline field with. + InsBasicConfigurationRegisterVn300( + protocol::uart::Scenario scenarioIn, + bool ahrsAidingIn, + bool estBaselineIn) : + scenario(scenarioIn), + ahrsAiding(ahrsAidingIn), + estBaseline(estBaselineIn) + { } + +}; + +/// \brief Structure representing the INS Advanced Configuration register. +struct InsAdvancedConfigurationRegister +{ + bool useMag; ///< The useMag field. + bool usePres; ///< The usePres field. + bool posAtt; ///< The posAtt field. + bool velAtt; ///< The velAtt field. + bool velBias; ///< The velBias field. + protocol::uart::FoamInit useFoam; ///< The useFoam field. + uint8_t gpsCovType; ///< The gpsCovType field. + uint8_t velCount; ///< The velCount field. + float velInit; ///< The velInit field. + float moveOrigin; ///< The moveOrigin field. + float gpsTimeout; ///< The gpsTimeout field. + float deltaLimitPos; ///< The deltaLimitPos field. + float deltaLimitVel; ///< The deltaLimitVel field. + float minPosUncertainty; ///< The minPosUncertainty field. + float minVelUncertainty; ///< The minVelUncertainty field. + + InsAdvancedConfigurationRegister() { } + + /// \brief Creates an initializes a new InsAdvancedConfigurationRegister structure. + /// + /// \param[in] useMagIn Value to initialize the useMag field with. + /// \param[in] usePresIn Value to initialize the usePres field with. + /// \param[in] posAttIn Value to initialize the posAtt field with. + /// \param[in] velAttIn Value to initialize the velAtt field with. + /// \param[in] velBiasIn Value to initialize the velBias field with. + /// \param[in] useFoamIn Value to initialize the useFoam field with. + /// \param[in] gpsCovTypeIn Value to initialize the gpsCovType field with. + /// \param[in] velCountIn Value to initialize the velCount field with. + /// \param[in] velInitIn Value to initialize the velInit field with. + /// \param[in] moveOriginIn Value to initialize the moveOrigin field with. + /// \param[in] gpsTimeoutIn Value to initialize the gpsTimeout field with. + /// \param[in] deltaLimitPosIn Value to initialize the deltaLimitPos field with. + /// \param[in] deltaLimitVelIn Value to initialize the deltaLimitVel field with. + /// \param[in] minPosUncertaintyIn Value to initialize the minPosUncertainty field with. + /// \param[in] minVelUncertaintyIn Value to initialize the minVelUncertainty field with. + InsAdvancedConfigurationRegister( + bool useMagIn, + bool usePresIn, + bool posAttIn, + bool velAttIn, + bool velBiasIn, + protocol::uart::FoamInit useFoamIn, + uint8_t gpsCovTypeIn, + uint8_t velCountIn, + float velInitIn, + float moveOriginIn, + float gpsTimeoutIn, + float deltaLimitPosIn, + float deltaLimitVelIn, + float minPosUncertaintyIn, + float minVelUncertaintyIn) : + useMag(useMagIn), + usePres(usePresIn), + posAtt(posAttIn), + velAtt(velAttIn), + velBias(velBiasIn), + useFoam(useFoamIn), + gpsCovType(gpsCovTypeIn), + velCount(velCountIn), + velInit(velInitIn), + moveOrigin(moveOriginIn), + gpsTimeout(gpsTimeoutIn), + deltaLimitPos(deltaLimitPosIn), + deltaLimitVel(deltaLimitVelIn), + minPosUncertainty(minPosUncertaintyIn), + minVelUncertainty(minVelUncertaintyIn) + { } + +}; + +/// \brief Structure representing the INS State - LLA register. +struct InsStateLlaRegister +{ + math::vec3f yawPitchRoll; ///< The yawPitchRoll field. + math::vec3d position; ///< The position field. + math::vec3f velocity; ///< The velocity field. + math::vec3f accel; ///< The accel field. + math::vec3f angularRate; ///< The angularRate field. + + InsStateLlaRegister() { } + + /// \brief Creates an initializes a new InsStateLlaRegister structure. + /// + /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. + /// \param[in] positionIn Value to initialize the position field with. + /// \param[in] velocityIn Value to initialize the velocity field with. + /// \param[in] accelIn Value to initialize the accel field with. + /// \param[in] angularRateIn Value to initialize the angularRate field with. + InsStateLlaRegister( + math::vec3f yawPitchRollIn, + math::vec3d positionIn, + math::vec3f velocityIn, + math::vec3f accelIn, + math::vec3f angularRateIn) : + yawPitchRoll(yawPitchRollIn), + position(positionIn), + velocity(velocityIn), + accel(accelIn), + angularRate(angularRateIn) + { } + +}; + +/// \brief Structure representing the INS State - ECEF register. +struct InsStateEcefRegister +{ + math::vec3f yawPitchRoll; ///< The yawPitchRoll field. + math::vec3d position; ///< The position field. + math::vec3f velocity; ///< The velocity field. + math::vec3f accel; ///< The accel field. + math::vec3f angularRate; ///< The angularRate field. + + InsStateEcefRegister() { } + + /// \brief Creates an initializes a new InsStateEcefRegister structure. + /// + /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. + /// \param[in] positionIn Value to initialize the position field with. + /// \param[in] velocityIn Value to initialize the velocity field with. + /// \param[in] accelIn Value to initialize the accel field with. + /// \param[in] angularRateIn Value to initialize the angularRate field with. + InsStateEcefRegister( + math::vec3f yawPitchRollIn, + math::vec3d positionIn, + math::vec3f velocityIn, + math::vec3f accelIn, + math::vec3f angularRateIn) : + yawPitchRoll(yawPitchRollIn), + position(positionIn), + velocity(velocityIn), + accel(accelIn), + angularRate(angularRateIn) + { } + +}; + +/// \brief Structure representing the Startup Filter Bias Estimate register. +struct StartupFilterBiasEstimateRegister +{ + math::vec3f gyroBias; ///< The gyroBias field. + math::vec3f accelBias; ///< The accelBias field. + float pressureBias; ///< The pressureBias field. + + StartupFilterBiasEstimateRegister() { } + + /// \brief Creates an initializes a new StartupFilterBiasEstimateRegister structure. + /// + /// \param[in] gyroBiasIn Value to initialize the gyroBias field with. + /// \param[in] accelBiasIn Value to initialize the accelBias field with. + /// \param[in] pressureBiasIn Value to initialize the pressureBias field with. + StartupFilterBiasEstimateRegister( + math::vec3f gyroBiasIn, + math::vec3f accelBiasIn, + float pressureBiasIn) : + gyroBias(gyroBiasIn), + accelBias(accelBiasIn), + pressureBias(pressureBiasIn) + { } + +}; + +/// \brief Structure representing the Delta Theta and Delta Velocity register. +struct DeltaThetaAndDeltaVelocityRegister +{ + float deltaTime; ///< The deltaTime field. + math::vec3f deltaTheta; ///< The deltaTheta field. + math::vec3f deltaVelocity; ///< The deltaVelocity field. + + DeltaThetaAndDeltaVelocityRegister() { } + + /// \brief Creates an initializes a new DeltaThetaAndDeltaVelocityRegister structure. + /// + /// \param[in] deltaTimeIn Value to initialize the deltaTime field with. + /// \param[in] deltaThetaIn Value to initialize the deltaTheta field with. + /// \param[in] deltaVelocityIn Value to initialize the deltaVelocity field with. + DeltaThetaAndDeltaVelocityRegister( + float deltaTimeIn, + math::vec3f deltaThetaIn, + math::vec3f deltaVelocityIn) : + deltaTime(deltaTimeIn), + deltaTheta(deltaThetaIn), + deltaVelocity(deltaVelocityIn) + { } + +}; + +/// \brief Structure representing the Delta Theta and Delta Velocity Configuration register. +struct DeltaThetaAndDeltaVelocityConfigurationRegister +{ + protocol::uart::IntegrationFrame integrationFrame; ///< The integrationFrame field. + protocol::uart::CompensationMode gyroCompensation; ///< The gyroCompensation field. + protocol::uart::CompensationMode accelCompensation; ///< The accelCompensation field. + + DeltaThetaAndDeltaVelocityConfigurationRegister() { } + + /// \brief Creates an initializes a new DeltaThetaAndDeltaVelocityConfigurationRegister structure. + /// + /// \param[in] integrationFrameIn Value to initialize the integrationFrame field with. + /// \param[in] gyroCompensationIn Value to initialize the gyroCompensation field with. + /// \param[in] accelCompensationIn Value to initialize the accelCompensation field with. + DeltaThetaAndDeltaVelocityConfigurationRegister( + protocol::uart::IntegrationFrame integrationFrameIn, + protocol::uart::CompensationMode gyroCompensationIn, + protocol::uart::CompensationMode accelCompensationIn) : + integrationFrame(integrationFrameIn), + gyroCompensation(gyroCompensationIn), + accelCompensation(accelCompensationIn) + { } + +}; + +/// \brief Structure representing the Reference Vector Configuration register. +struct ReferenceVectorConfigurationRegister +{ + bool useMagModel; ///< The useMagModel field. + bool useGravityModel; ///< The useGravityModel field. + uint32_t recalcThreshold; ///< The recalcThreshold field. + float year; ///< The year field. + math::vec3d position; ///< The position field. + + ReferenceVectorConfigurationRegister() { } + + /// \brief Creates an initializes a new ReferenceVectorConfigurationRegister structure. + /// + /// \param[in] useMagModelIn Value to initialize the useMagModel field with. + /// \param[in] useGravityModelIn Value to initialize the useGravityModel field with. + /// \param[in] recalcThresholdIn Value to initialize the recalcThreshold field with. + /// \param[in] yearIn Value to initialize the year field with. + /// \param[in] positionIn Value to initialize the position field with. + ReferenceVectorConfigurationRegister( + bool useMagModelIn, + bool useGravityModelIn, + uint32_t recalcThresholdIn, + float yearIn, + math::vec3d positionIn) : + useMagModel(useMagModelIn), + useGravityModel(useGravityModelIn), + recalcThreshold(recalcThresholdIn), + year(yearIn), + position(positionIn) + { } + +}; + +/// \brief Structure representing the Gyro Compensation register. +struct GyroCompensationRegister +{ + math::mat3f c; ///< The c field. + math::vec3f b; ///< The b field. + + GyroCompensationRegister() { } + + /// \brief Creates an initializes a new GyroCompensationRegister structure. + /// + /// \param[in] cIn Value to initialize the c field with. + /// \param[in] bIn Value to initialize the b field with. + GyroCompensationRegister( + math::mat3f cIn, + math::vec3f bIn) : + c(cIn), + b(bIn) + { } + +}; + +/// \brief Structure representing the IMU Filtering Configuration register. +struct ImuFilteringConfigurationRegister +{ + uint16_t magWindowSize; ///< The magWindowSize field. + uint16_t accelWindowSize; ///< The accelWindowSize field. + uint16_t gyroWindowSize; ///< The gyroWindowSize field. + uint16_t tempWindowSize; ///< The tempWindowSize field. + uint16_t presWindowSize; ///< The presWindowSize field. + protocol::uart::FilterMode magFilterMode; ///< The magFilterMode field. + protocol::uart::FilterMode accelFilterMode; ///< The accelFilterMode field. + protocol::uart::FilterMode gyroFilterMode; ///< The gyroFilterMode field. + protocol::uart::FilterMode tempFilterMode; ///< The tempFilterMode field. + protocol::uart::FilterMode presFilterMode; ///< The presFilterMode field. + + ImuFilteringConfigurationRegister() { } + + /// \brief Creates an initializes a new ImuFilteringConfigurationRegister structure. + /// + /// \param[in] magWindowSizeIn Value to initialize the magWindowSize field with. + /// \param[in] accelWindowSizeIn Value to initialize the accelWindowSize field with. + /// \param[in] gyroWindowSizeIn Value to initialize the gyroWindowSize field with. + /// \param[in] tempWindowSizeIn Value to initialize the tempWindowSize field with. + /// \param[in] presWindowSizeIn Value to initialize the presWindowSize field with. + /// \param[in] magFilterModeIn Value to initialize the magFilterMode field with. + /// \param[in] accelFilterModeIn Value to initialize the accelFilterMode field with. + /// \param[in] gyroFilterModeIn Value to initialize the gyroFilterMode field with. + /// \param[in] tempFilterModeIn Value to initialize the tempFilterMode field with. + /// \param[in] presFilterModeIn Value to initialize the presFilterMode field with. + ImuFilteringConfigurationRegister( + uint16_t magWindowSizeIn, + uint16_t accelWindowSizeIn, + uint16_t gyroWindowSizeIn, + uint16_t tempWindowSizeIn, + uint16_t presWindowSizeIn, + protocol::uart::FilterMode magFilterModeIn, + protocol::uart::FilterMode accelFilterModeIn, + protocol::uart::FilterMode gyroFilterModeIn, + protocol::uart::FilterMode tempFilterModeIn, + protocol::uart::FilterMode presFilterModeIn) : + magWindowSize(magWindowSizeIn), + accelWindowSize(accelWindowSizeIn), + gyroWindowSize(gyroWindowSizeIn), + tempWindowSize(tempWindowSizeIn), + presWindowSize(presWindowSizeIn), + magFilterMode(magFilterModeIn), + accelFilterMode(accelFilterModeIn), + gyroFilterMode(gyroFilterModeIn), + tempFilterMode(tempFilterModeIn), + presFilterMode(presFilterModeIn) + { } + +}; + +/// \brief Structure representing the GPS Compass Baseline register. +struct GpsCompassBaselineRegister +{ + math::vec3f position; ///< The position field. + math::vec3f uncertainty; ///< The uncertainty field. + + GpsCompassBaselineRegister() { } + + /// \brief Creates an initializes a new GpsCompassBaselineRegister structure. + /// + /// \param[in] positionIn Value to initialize the position field with. + /// \param[in] uncertaintyIn Value to initialize the uncertainty field with. + GpsCompassBaselineRegister( + math::vec3f positionIn, + math::vec3f uncertaintyIn) : + position(positionIn), + uncertainty(uncertaintyIn) + { } + +}; + +/// \brief Structure representing the GPS Compass Estimated Baseline register. +struct GpsCompassEstimatedBaselineRegister +{ + bool estBaselineUsed; ///< The estBaselineUsed field. + uint16_t numMeas; ///< The numMeas field. + math::vec3f position; ///< The position field. + math::vec3f uncertainty; ///< The uncertainty field. + + GpsCompassEstimatedBaselineRegister() { } + + /// \brief Creates an initializes a new GpsCompassEstimatedBaselineRegister structure. + /// + /// \param[in] estBaselineUsedIn Value to initialize the estBaselineUsed field with. + /// \param[in] numMeasIn Value to initialize the numMeas field with. + /// \param[in] positionIn Value to initialize the position field with. + /// \param[in] uncertaintyIn Value to initialize the uncertainty field with. + GpsCompassEstimatedBaselineRegister( + bool estBaselineUsedIn, + uint16_t numMeasIn, + math::vec3f positionIn, + math::vec3f uncertaintyIn) : + estBaselineUsed(estBaselineUsedIn), + numMeas(numMeasIn), + position(positionIn), + uncertainty(uncertaintyIn) + { } + +}; + +/// \brief Structure representing the IMU Rate Configuration register. +struct ImuRateConfigurationRegister +{ + uint16_t imuRate; ///< The imuRate field. + uint16_t navDivisor; ///< The navDivisor field. + float filterTargetRate; ///< The filterTargetRate field. + float filterMinRate; ///< The filterMinRate field. + + ImuRateConfigurationRegister() { } + + /// \brief Creates an initializes a new ImuRateConfigurationRegister structure. + /// + /// \param[in] imuRateIn Value to initialize the imuRate field with. + /// \param[in] navDivisorIn Value to initialize the navDivisor field with. + /// \param[in] filterTargetRateIn Value to initialize the filterTargetRate field with. + /// \param[in] filterMinRateIn Value to initialize the filterMinRate field with. + ImuRateConfigurationRegister( + uint16_t imuRateIn, + uint16_t navDivisorIn, + float filterTargetRateIn, + float filterMinRateIn) : + imuRate(imuRateIn), + navDivisor(navDivisorIn), + filterTargetRate(filterTargetRateIn), + filterMinRate(filterMinRateIn) + { } + +}; + +/// \brief Structure representing the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register. +struct YawPitchRollTrueBodyAccelerationAndAngularRatesRegister +{ + math::vec3f yawPitchRoll; ///< The yawPitchRoll field. + math::vec3f bodyAccel; ///< The bodyAccel field. + math::vec3f gyro; ///< The gyro field. + + YawPitchRollTrueBodyAccelerationAndAngularRatesRegister() { } + + /// \brief Creates an initializes a new YawPitchRollTrueBodyAccelerationAndAngularRatesRegister structure. + /// + /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. + /// \param[in] bodyAccelIn Value to initialize the bodyAccel field with. + /// \param[in] gyroIn Value to initialize the gyro field with. + YawPitchRollTrueBodyAccelerationAndAngularRatesRegister( + math::vec3f yawPitchRollIn, + math::vec3f bodyAccelIn, + math::vec3f gyroIn) : + yawPitchRoll(yawPitchRollIn), + bodyAccel(bodyAccelIn), + gyro(gyroIn) + { } + +}; + +/// \brief Structure representing the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register. +struct YawPitchRollTrueInertialAccelerationAndAngularRatesRegister +{ + math::vec3f yawPitchRoll; ///< The yawPitchRoll field. + math::vec3f inertialAccel; ///< The inertialAccel field. + math::vec3f gyro; ///< The gyro field. + + YawPitchRollTrueInertialAccelerationAndAngularRatesRegister() { } + + /// \brief Creates an initializes a new YawPitchRollTrueInertialAccelerationAndAngularRatesRegister structure. + /// + /// \param[in] yawPitchRollIn Value to initialize the yawPitchRoll field with. + /// \param[in] inertialAccelIn Value to initialize the inertialAccel field with. + /// \param[in] gyroIn Value to initialize the gyro field with. + YawPitchRollTrueInertialAccelerationAndAngularRatesRegister( + math::vec3f yawPitchRollIn, + math::vec3f inertialAccelIn, + math::vec3f gyroIn) : + yawPitchRoll(yawPitchRollIn), + inertialAccel(inertialAccelIn), + gyro(gyroIn) + { } + +}; + +/// \} + +} +} + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/sensors/searcher.h b/ext/vnproglib-1.1.0.115/include/vn/sensors/searcher.h new file mode 100755 index 0000000..ebc8bb1 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/sensors/searcher.h @@ -0,0 +1,55 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifndef _VNSENSORS_SEARCHER_H_ +#define _VNSENSORS_SEARCHER_H_ + +#include +#include + +#include "vn/int.h" +#include "vn/util/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 true if a sensor if found; otherwise false. + static bool search(const std::string &portName, uint32_t *foundBaudrate); + + /// \brief Checks all available serial ports on the system for any + /// VectorNav sensors. + /// + /// \return Collection of serial ports and baudrates for all found sensors. + static std::vector > search(void); + + /// \brief Checks the provided list of serial ports for any connected + /// VectorNav sensors. + /// + /// \param[in] portsToCheck List of serial ports to check for sensors. + /// \return Collection of serial ports and baudrates for all found sensors. + static std::vector > search(std::vector portsToCheck); + + /// \brief Tests if a sensor is connected to the serial port at the + /// specified baudrate. + /// + /// \param[in] portName The serial port to test. + /// \param[in] baudrate The baudrate to test at. + /// \returns true if a sensor if found; otherwise false. + static bool test(std::string portName, uint32_t baudrate); +}; + +} +} + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/sensors/sensors.h b/ext/vnproglib-1.1.0.115/include/vn/sensors/sensors.h new file mode 100755 index 0000000..1fae875 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/sensors/sensors.h @@ -0,0 +1,1436 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifndef _VNSENSORS_SENSORS_H_ +#define _VNSENSORS_SENSORS_H_ + +#if PYTHON + #include "vn/util/boostpython.h" +#endif + +#include +#include + +#include "vn/int.h" +#include "vn/util/nocopy.h" +#include "vn/protocol/uart/packetfinder.h" +#include "vn/util/export.h" +#include "vn/sensors/registers.h" + +#if PYTHON + #include "vn/event.h" +#endif + +namespace vn { + +namespace xplat { + class IPort; +} +namespace sensors { + +/// \brief Represents an error from a VectorNav sensor. +struct sensor_error : public std::exception +{ +private: + sensor_error(); + +public: + + /// \brief Creates a new sensor_error based on the error value provided by + /// the sensor. + explicit sensor_error(protocol::uart::SensorError e); + + /// \brief Copy constructor. + sensor_error(const sensor_error& e); + + ~sensor_error() throw(); + + /// \brief Returns a description of the exception. + /// + /// \return A description of the exception. + char const* what() const throw(); + + /// \brief The associated sensor error. + protocol::uart::SensorError error; + +private: + char *_errorMessage; +}; + +/// \brief Helpful class for working with VectorNav sensors. +class vn_proglib_DLLEXPORT VnSensor : private util::NoCopy +{ + +public: + + enum Family + { + VnSensor_Family_Unknown, ///< Unknown device family. + VnSensor_Family_Vn100, ///< A device of the VectorNav VN-100 sensor family. + VnSensor_Family_Vn200, ///< A device of the VectorNav VN-200 sensor family. + VnSensor_Family_Vn300 ///< A device of the VectorNav VN-300 sensor family. + }; + + #if PYTHON + typedef Event AsyncPacketReceivedEvent; + #endif + + /// \brief Defines a callback handler that can received notification when + /// the VnSensor receives raw data from its port. + /// + /// \param[in] userData Pointer to user data that was initially supplied + /// when the callback was registered via registerRawDataReceivedHandler. + /// \param[in] rawData Pointer to the raw data. + /// \param[in] length The number of bytes of raw data. + /// \param[in] runningIndex The running index of the received data. + typedef void(*RawDataReceivedHandler)(void* userData, const char* rawData, size_t length, size_t runningIndex); + + /// \brief Defines the signature for a method that can receive + /// notifications of new possible packets found. + /// + /// \param[in] userData Pointer to user data that was initially supplied + /// when the callback was registered via registerPossiblePacketFoundHandler. + /// \param[in] possiblePacket The possible packet that was found. + /// \param[in] packetStartRunningIndex The running index of the start of + /// the packet. + typedef void(*PossiblePacketFoundHandler)(void* userData, protocol::uart::Packet& possiblePacket, size_t packetStartRunningIndex); + + /// \brief Defines the signature for a method that can receive + /// notifications of when a new asynchronous data packet (ASCII or BINARY) + /// is received. + /// + /// This packet will have already had and pertinent error checking + /// performed and determined to be an asynchronous packet. + /// + /// \param[in] userData Pointer to user data that was initially supplied + /// when the callback was registered via registerAsyncPacketReceivedHandler. + /// \param[in] asyncPacket The asynchronous packet received. + /// \param[in] packetStartRunningIndex The running index of the start of + /// the packet. + typedef void(*AsyncPacketReceivedHandler)(void* userData, protocol::uart::Packet& asyncPacket, size_t packetStartRunningIndex); + + /// \brief Defines the signature for a method that can receive + /// notifications when an error message is received. + /// + /// This packet will have already had and pertinent error checking + /// performed and determined to be an asynchronous packet. + /// + /// \param[in] userData Pointer to user data that was initially supplied + /// when the callback was registered via registerErrorPacketReceivedHandler. + /// \param[in] errorPacket The error packet received. + /// \param[in] packetStartRunningIndex The running index of the start of + /// the packet. + typedef void(*ErrorPacketReceivedHandler)(void* userData, protocol::uart::Packet& errorPacket, size_t packetStartRunningIndex); + + /// \brief The list of baudrates supported by VectorNav sensors. + static std::vector supportedBaudrates(); + + VnSensor(); + + ~VnSensor(); + + /// \brief Returns the baudrate of the serial port connection. Note this + /// is independent of the sensor's on-board serial baudrate setting. + /// + /// \return The connected baudrate. + uint32_t baudrate(); + + /// \defgroup vnSensorProperties VnSensor Properties + /// \brief This group of methods interface with the VnSensor properties. + /// + /// \{ + + /// \brief Gets the current error detection mode used for commands sent to + /// the sensor. Default is protocol::uart::ErrorDetectionMode::CHECKSUM. + /// + /// \return The error detection mode used for packets sent to the sensor. + protocol::uart::ErrorDetectionMode sendErrorDetectionMode(); + + /// \brief Sets the error detection mode used by the class for commands + /// sent to the sensor. + /// + /// \param[in] mode The new error detection mode for packets sent to the + /// sensor. + void setSendErrorDetectionMode(protocol::uart::ErrorDetectionMode mode); + + /// \brief Indicates if the VnSensor is connected. + /// + /// \return true if the VnSensor is connected; otherwise false. + bool isConnected(); + + /// \brief Gets the amount of time in milliseconds to wait for a response + /// during read/writes with the sensor. + /// + /// \return The response timeout in milliseconds. + uint16_t responseTimeoutMs(); + + /// \brief Sets the amount of time in milliseconds to wait for a response + /// during read/writes with the sensor. + /// + /// \param[in] timeout The number of milliseconds for response timeouts. + void setResponseTimeoutMs(uint16_t timeout); + + /// \brief The delay in milliseconds between retransmitting commands. + /// + /// \return The retransmit delay in milliseconds. + uint16_t retransmitDelayMs(); + + /// \brief Sets the delay in milliseconds between command retransmits. + /// + /// \param[in] delay The retransmit delay in milliseconds. + void setRetransmitDelayMs(uint16_t delay); + + /// \} + + /// \brief Checks if we are able to send and receive communication with a sensor. + /// + /// \return true if we can communicate with the sensor; otherwise false. + bool verifySensorConnectivity(); + + /// \brief Connects to a VectorNav sensor. + /// + /// \param[in] portName The name of the serial port to connect to. + /// \param[in] baudrate The baudrate to connect at. + void connect(const std::string &portName, uint32_t baudrate); + + /// \brief Allows connecting to a VectorNav sensor over a + /// \ref vn::common::ISimplePort. + /// + /// The caller is responsible for properly destroying the + /// \ref vn::common::ISimplePort object when this method is used. Also, if + /// the provided \ref vn::common::ISimplePort is already open, then when + /// the method \ref disconnect is called, \ref VnSensor will not attempt to + /// close the port. However, if the \ref vn::common::ISimplePort is not + /// open, then any subsequent calls to \ref disconnect will close the port. + /// + /// \param[in] simplePort An \ref vn::common::ISimplePort. May be either + /// currently open or closed. + void connect(xplat::IPort* port); + + /// \brief Disconnects from the VectorNav sensor. + /// + /// \exception invalid_operation Thrown if the VnSensor is not + /// connected. + void disconnect(); + + /// \brief Sends the provided command and returns the response from the sensor. + /// + /// If the command does not have an asterisk '*', then a checksum will be performed + /// and appended based on the current error detection mode. Also, if the line-ending + /// \\r\\n is not present, these will be added also. + /// + /// \param[in] toSend The command to send to the sensor. + /// \return The response received from the sensor. + std::string transaction(std::string toSend); + + /// \brief Writes a raw data string to the sensor, normally appending an + /// appropriate error detection checksum. + /// + /// No checksum is necessary as any missing checksum will be provided. For + /// example, the following toSend data will be correctly received by the + /// sensor. + /// - $VNRRG,1*42\\r\\n + /// - $VNRRG,1*42 + /// - $VNRRG,1* + /// - $VNRRG,1 + /// - VNRRG,1 + /// + /// If waitForReply is true, then the method will wait for a + /// response and return the received response. Otherwise, if false, + /// the method will send the data and return immediately with an empty + /// string. + /// + /// \param[in] toSend The data to send. The method will automatically + /// append a checksum/CRC if none is provided. + /// \param[in] waitForReply Indicates if the method should wait for a + /// response and return the received response. + /// \param[in] errorDetectionMode Indicates the error detection mode to + /// append to any packets to send. + /// \return The received response if waitForReply is true; otherwise + /// this will be an empty string. + std::string send( + std::string toSend, + bool waitForReply = true, + protocol::uart::ErrorDetectionMode errorDetectionMode = protocol::uart::ERRORDETECTIONMODE_CHECKSUM); + + /// \brief Issues a tare command to the VectorNav Sensor. + /// + /// \param[in] waitForReply Indicates if the method should wait for a + /// response from the sensor. + void tare(bool waitForReply = true); + + /// \brief Issues a command to the VectorNav Sensor to set the gyro's bias. + /// + /// \param[in] waitForReply Indicates if the method should wait for a + /// response from the sensor. + void setGyroBias(bool waitForReply = true); + + /// \brief Command to inform the VectorNav Sensor if there is a magnetic disturbance present. + /// + /// \param[in] disturbancePresent Indicates the presence of a magnetic disturbance + /// \param[in] waitForReply Indicates if the method should wait for a + /// response from the sensor. + void magneticDisturbancePresent(bool disturbancePresent, bool waitForReply = true); + + /// \brief Command to inform the VectorNav Sensor if there is an acceleration disturbance present. + /// + /// \param[in] disturbancePresent Indicates the presense of an acceleration disturbance. + /// \param[in] waitForReply Indicates if the method should wait for a + /// response from the sensor. + void accelerationDisturbancePresent(bool disturbancePresent, bool waitForReply = true); + + /// \brief Issues a Write Settings command to the VectorNav Sensor. + /// + /// \param[in] waitForReply Indicates if the method should wait for a + /// response from the sensor. + void writeSettings(bool waitForReply = true); + + /// \brief Issues a Restore Factory Settings command to the VectorNav + /// sensor. + /// + /// \param[in] waitForReply Indicates if the method should wait for a + /// response from the sensor. + void restoreFactorySettings(bool waitForReply = true); + + /// \brief Issues a Reset command to the VectorNav sensor. + /// + /// \param[in] waitForReply Indicates if the method should wait for a + /// response from the sensor. + void reset(bool waitForReply = true); + + /// \brief Issues a change baudrate to the VectorNav sensor and then + /// reconnects the attached serial port at the new baudrate. + /// + /// \param[in] baudrate The new sensor baudrate. + void changeBaudRate(uint32_t baudrate); + + /// \brief This method will connect to the specified serial port, query the + /// attached sensor, and determine the type of device. + /// + /// \param[in] portName The COM port name to connect to. + /// \param[in] baudrate The baudrate to connect at. + //static void determineDeviceFamily(std::string portName, uint32_t baudrate); + + /// \brief This method will query the attached sensor and determine the + /// device family it belongs to. + /// + /// \return The determined device family. + Family determineDeviceFamily(); + + /// \brief This method determines which device family a VectorNav sensor + /// belongs to based on the provided model number. + /// + /// \return The determined device family. + static Family determineDeviceFamily(std::string modelNumber); + + /// \defgroup vnSensorEvents VnSensor Events + /// \brief This group of methods allow registering/unregistering for events + /// of the VnSensor. + /// + /// \{ + + /// \brief Registers a callback method for notification when raw data is + /// received. + /// + /// \param[in] userData Pointer to user data, which will be provided to the + /// callback method. + /// \param[in] handler The callback method. + void registerRawDataReceivedHandler(void* userData, RawDataReceivedHandler handler); + + #if PL150 + //Event event + #else + #if PYTHON + + PyObject* registerRawDataReceivedHandler(PyObject* callable); + + #endif + #endif + + /// \brief Unregisters the registered callback method. + void unregisterRawDataReceivedHandler(); + + /// \brief Registers a callback method for notification when a new possible + /// packet is found. + /// + /// \param[in] userData Pointer to user data, which will be provided to the + /// callback method. + /// \param[in] handler The callback method. + void registerPossiblePacketFoundHandler(void* userData, PossiblePacketFoundHandler handler); + + /// \brief Unregisters the registered callback method. + void unregisterPossiblePacketFoundHandler(); + + /// \brief Registers a callback method for notification when a new + /// asynchronous data packet is received. + /// + /// \param[in] userData Pointer to user data, which will be provided to the + /// callback method. + /// \param[in] handler The callback method. + void registerAsyncPacketReceivedHandler(void* userData, AsyncPacketReceivedHandler handler); + + #if PL150 + //packet, index, timestamp + //Event eventAsyncPacketRecieved; + #if PYTHON + AsyncPacketReceivedEvent eventAsyncPacketReceived; + #endif + + #else + #if PYTHON + PyObject* registerAsyncPacketReceivedHandler(PyObject* callable); + #endif + #endif + + /// \brief Unregisters the registered callback method. + void unregisterAsyncPacketReceivedHandler(); + + /// \brief Registers a callback method for notification when an error + /// packet is received. + /// + /// \param[in] userData Pointer to user data, which will be provided to the + /// callback method. + /// \param[in] handler The callback method. + void registerErrorPacketReceivedHandler(void* userData, ErrorPacketReceivedHandler handler); + + /// \brief Unregisters the registered callback method. + void unregisterErrorPacketReceivedHandler(); + + /// \} + + /// \defgroup registerAccessMethods Register Access Methods + /// \brief This group of methods provide access to read and write to the + /// sensor's registers. + /// + /// \{ + + /// \brief Reads the Binary Output 1 register. + /// + /// \return The register's values. + BinaryOutputRegister readBinaryOutput1(); + + /// \brief Writes to the Binary Output 1 register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeBinaryOutput1(BinaryOutputRegister &fields, bool waitForReply = true); + + /// \brief Reads the Binary Output 2 register. + /// + /// \return The register's values. + BinaryOutputRegister readBinaryOutput2(); + + /// \brief Writes to the Binary Output 2 register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeBinaryOutput2(BinaryOutputRegister &fields, bool waitForReply = true); + + /// \brief Reads the Binary Output 3 register. + /// + /// \return The register's values. + BinaryOutputRegister readBinaryOutput3(); + + /// \brief Writes to the Binary Output 3 register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeBinaryOutput3(BinaryOutputRegister &fields, bool waitForReply = true); + + + /// \brief Reads the Serial Baud Rate register for the specified port. + /// + /// \param[in] port The port number to read from. + /// \return The register's values. + uint32_t readSerialBaudRate(uint8_t port); + + /// \brief Writes to the Serial Baud Rate register for the specified port. + /// + /// \param[in] baudrate The register's Baud Rate field. + /// \param[in] port The port number to write to. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeSerialBaudRate(const uint32_t &baudrate, uint8_t port, bool waitForReply = true); + + /// \brief Reads the Async Data Output Type register. + /// + /// \param[in] port The port number to read from. + /// \return The register's values. + protocol::uart::AsciiAsync readAsyncDataOutputType(uint8_t port); + + /// \brief Writes to the Async Data Output Type register. + /// + /// \param[in] ador The register's ADOR field. + /// \param[in] port The port number to write to. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeAsyncDataOutputType(protocol::uart::AsciiAsync ador, uint8_t port, bool waitForReply = true); + + /// \brief Reads the Async Data Output Frequency register. + /// + /// \param[in] port The port number to read from. + /// \return The register's values. + uint32_t readAsyncDataOutputFrequency(uint8_t port); + + /// \brief Writes to the Async Data Output Frequency register. + /// + /// \param[in] adof The register's ADOF field. + /// \param[in] port The port number to write to. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeAsyncDataOutputFrequency(const uint32_t &adof, uint8_t port, bool waitForReply = true); + + /// \brief Reads the INS Basic Configuration register for a VN-200 sensor. + /// + /// \return The register's values. + InsBasicConfigurationRegisterVn200 readInsBasicConfigurationVn200(); + + /// \brief Writes to the INS Basic Configuration register for a VN-200 sensor. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeInsBasicConfigurationVn200(InsBasicConfigurationRegisterVn200 &fields, bool waitForReply = true); + + /// \brief Writes to the INS Basic Configuration register for a VN-200 sensor. + /// + /// \param[in] scenario Value for the Scenario field. + /// \param[in] ahrsAiding Value for the AhrsAiding field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeInsBasicConfigurationVn200( + protocol::uart::Scenario scenario, + const uint8_t &ahrsAiding, + bool waitForReply = true); + + /// \brief Reads the INS Basic Configuration register for a VN-300 sensor. + /// + /// \return The register's values. + InsBasicConfigurationRegisterVn300 readInsBasicConfigurationVn300(); + + /// \brief Writes to the INS Basic Configuration register for a VN-300 sensor. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeInsBasicConfigurationVn300(InsBasicConfigurationRegisterVn300 &fields, bool waitForReply = true); + + /// \brief Writes to the INS Basic Configuration register for a VN-300 sensor. + /// + /// \param[in] scenario Value for the Scenario field. + /// \param[in] ahrsAiding Value for the AhrsAiding field. + /// \param[in] estBaseline Value for the EstBaseline field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeInsBasicConfigurationVn300( + protocol::uart::Scenario scenario, + const uint8_t &ahrsAiding, + const uint8_t &estBaseline, + bool waitForReply = true); + + /// \brief Reads the User Tag register. + /// + /// \return The register's values. + std::string readUserTag(); + + /// \brief Writes to the User Tag register. + /// + /// \param[in] tag The register's Tag field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeUserTag(const std::string &tag, bool waitForReply = true); + + /// \brief Reads the Model Number register. + /// + /// \return The register's values. + std::string readModelNumber(); + + /// \brief Reads the Hardware Revision register. + /// + /// \return The register's values. + uint32_t readHardwareRevision(); + + /// \brief Reads the Serial Number register. + /// + /// \return The register's values. + uint32_t readSerialNumber(); + + /// \brief Reads the Firmware Version register. + /// + /// \return The register's values. + std::string readFirmwareVersion(); + + /// \brief Reads the Serial Baud Rate register. + /// + /// \return The register's values. + uint32_t readSerialBaudRate(); + + /// \brief Writes to the Serial Baud Rate register. + /// + /// \param[in] baudrate The register's Baud Rate field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeSerialBaudRate(const uint32_t &baudrate, bool waitForReply = true); + + /// \brief Reads the Async Data Output Type register. + /// + /// \return The register's values. + protocol::uart::AsciiAsync readAsyncDataOutputType(); + + /// \brief Writes to the Async Data Output Type register. + /// + /// \param[in] ador The register's ADOR field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeAsyncDataOutputType(protocol::uart::AsciiAsync ador, bool waitForReply = true); + + /// \brief Reads the Async Data Output Frequency register. + /// + /// \return The register's values. + uint32_t readAsyncDataOutputFrequency(); + + /// \brief Writes to the Async Data Output Frequency register. + /// + /// \param[in] adof The register's ADOF field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeAsyncDataOutputFrequency(const uint32_t &adof, bool waitForReply = true); + + /// \brief Reads the Yaw Pitch Roll register. + /// + /// \return The register's values. + math::vec3f readYawPitchRoll(); + + /// \brief Reads the Attitude Quaternion register. + /// + /// \return The register's values. + math::vec4f readAttitudeQuaternion(); + + /// \brief Reads the Quaternion, Magnetic, Acceleration and Angular Rates register. + /// + /// \return The register's values. + QuaternionMagneticAccelerationAndAngularRatesRegister readQuaternionMagneticAccelerationAndAngularRates(); + + /// \brief Reads the Magnetic Measurements register. + /// + /// \return The register's values. + math::vec3f readMagneticMeasurements(); + + /// \brief Reads the Acceleration Measurements register. + /// + /// \return The register's values. + math::vec3f readAccelerationMeasurements(); + + /// \brief Reads the Angular Rate Measurements register. + /// + /// \return The register's values. + math::vec3f readAngularRateMeasurements(); + + /// \brief Reads the Magnetic, Acceleration and Angular Rates register. + /// + /// \return The register's values. + MagneticAccelerationAndAngularRatesRegister readMagneticAccelerationAndAngularRates(); + + /// \brief Reads the Magnetic and Gravity Reference Vectors register. + /// + /// \return The register's values. + MagneticAndGravityReferenceVectorsRegister readMagneticAndGravityReferenceVectors(); + + /// \brief Writes to the Magnetic and Gravity Reference Vectors register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeMagneticAndGravityReferenceVectors(MagneticAndGravityReferenceVectorsRegister &fields, bool waitForReply = true); + + /// \brief Writes to the Magnetic and Gravity Reference Vectors register. + /// + /// \param[in] magRef Value for the MagRef field. + /// \param[in] accRef Value for the AccRef field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeMagneticAndGravityReferenceVectors( + const math::vec3f &magRef, + const math::vec3f &accRef, + bool waitForReply = true); + + /// \brief Reads the Filter Measurements Variance Parameters register. + /// + /// \return The register's values. + FilterMeasurementsVarianceParametersRegister readFilterMeasurementsVarianceParameters(); + + /// \brief Writes to the Filter Measurements Variance Parameters register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeFilterMeasurementsVarianceParameters(FilterMeasurementsVarianceParametersRegister &fields, bool waitForReply = true); + + /// \brief Writes to the Filter Measurements Variance Parameters register. + /// + /// \param[in] angularWalkVariance Value for the Angular Walk Variance field. + /// \param[in] angularRateVariance Value for the Angular Rate Variance field. + /// \param[in] magneticVariance Value for the Magnetic Variance field. + /// \param[in] accelerationVariance Value for the Acceleration Variance field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeFilterMeasurementsVarianceParameters( + const float &angularWalkVariance, + const math::vec3f &angularRateVariance, + const math::vec3f &magneticVariance, + const math::vec3f &accelerationVariance, + bool waitForReply = true); + + /// \brief Reads the Magnetometer Compensation register. + /// + /// \return The register's values. + MagnetometerCompensationRegister readMagnetometerCompensation(); + + /// \brief Writes to the Magnetometer Compensation register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeMagnetometerCompensation(MagnetometerCompensationRegister &fields, bool waitForReply = true); + + /// \brief Writes to the Magnetometer Compensation register. + /// + /// \param[in] c Value for the C field. + /// \param[in] b Value for the B field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeMagnetometerCompensation( + const math::mat3f &c, + const math::vec3f &b, + bool waitForReply = true); + + /// \brief Reads the Filter Active Tuning Parameters register. + /// + /// \return The register's values. + FilterActiveTuningParametersRegister readFilterActiveTuningParameters(); + + /// \brief Writes to the Filter Active Tuning Parameters register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeFilterActiveTuningParameters(FilterActiveTuningParametersRegister &fields, bool waitForReply = true); + + /// \brief Writes to the Filter Active Tuning Parameters register. + /// + /// \param[in] magneticDisturbanceGain Value for the Magnetic Disturbance Gain field. + /// \param[in] accelerationDisturbanceGain Value for the Acceleration Disturbance Gain field. + /// \param[in] magneticDisturbanceMemory Value for the Magnetic Disturbance Memory field. + /// \param[in] accelerationDisturbanceMemory Value for the Acceleration Disturbance Memory field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeFilterActiveTuningParameters( + const float &magneticDisturbanceGain, + const float &accelerationDisturbanceGain, + const float &magneticDisturbanceMemory, + const float &accelerationDisturbanceMemory, + bool waitForReply = true); + + /// \brief Reads the Acceleration Compensation register. + /// + /// \return The register's values. + AccelerationCompensationRegister readAccelerationCompensation(); + + /// \brief Writes to the Acceleration Compensation register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeAccelerationCompensation(AccelerationCompensationRegister &fields, bool waitForReply = true); + + /// \brief Writes to the Acceleration Compensation register. + /// + /// \param[in] c Value for the C field. + /// \param[in] b Value for the B field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeAccelerationCompensation( + const math::mat3f &c, + const math::vec3f &b, + bool waitForReply = true); + + /// \brief Reads the Reference Frame Rotation register. + /// + /// \return The register's values. + math::mat3f readReferenceFrameRotation(); + + /// \brief Writes to the Reference Frame Rotation register. + /// + /// \param[in] c The register's C field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeReferenceFrameRotation(const math::mat3f &c, bool waitForReply = true); + + /// \brief Reads the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register. + /// + /// \return The register's values. + YawPitchRollMagneticAccelerationAndAngularRatesRegister readYawPitchRollMagneticAccelerationAndAngularRates(); + + /// \brief Reads the Communication Protocol Control register. + /// + /// \return The register's values. + CommunicationProtocolControlRegister readCommunicationProtocolControl(); + + /// \brief Writes to the Communication Protocol Control register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeCommunicationProtocolControl(CommunicationProtocolControlRegister &fields, bool waitForReply = true); + + /// \brief Writes to the Communication Protocol Control register. + /// + /// \param[in] serialCount Value for the SerialCount field. + /// \param[in] serialStatus Value for the SerialStatus field. + /// \param[in] spiCount Value for the SPICount field. + /// \param[in] spiStatus Value for the SPIStatus field. + /// \param[in] serialChecksum Value for the SerialChecksum field. + /// \param[in] spiChecksum Value for the SPIChecksum field. + /// \param[in] errorMode Value for the ErrorMode field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeCommunicationProtocolControl( + protocol::uart::CountMode serialCount, + protocol::uart::StatusMode serialStatus, + protocol::uart::CountMode spiCount, + protocol::uart::StatusMode spiStatus, + protocol::uart::ChecksumMode serialChecksum, + protocol::uart::ChecksumMode spiChecksum, + protocol::uart::ErrorMode errorMode, + bool waitForReply = true); + + /// \brief Reads the Synchronization Control register. + /// + /// \return The register's values. + SynchronizationControlRegister readSynchronizationControl(); + + /// \brief Writes to the Synchronization Control register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeSynchronizationControl(SynchronizationControlRegister &fields, bool waitForReply = true); + + /// \brief Writes to the Synchronization Control register. + /// + /// \param[in] syncInMode Value for the SyncInMode field. + /// \param[in] syncInEdge Value for the SyncInEdge field. + /// \param[in] syncInSkipFactor Value for the SyncInSkipFactor field. + /// \param[in] syncOutMode Value for the SyncOutMode field. + /// \param[in] syncOutPolarity Value for the SyncOutPolarity field. + /// \param[in] syncOutSkipFactor Value for the SyncOutSkipFactor field. + /// \param[in] syncOutPulseWidth Value for the SyncOutPulseWidth field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeSynchronizationControl( + protocol::uart::SyncInMode syncInMode, + protocol::uart::SyncInEdge syncInEdge, + const uint16_t &syncInSkipFactor, + protocol::uart::SyncOutMode syncOutMode, + protocol::uart::SyncOutPolarity syncOutPolarity, + const uint16_t &syncOutSkipFactor, + const uint32_t &syncOutPulseWidth, + bool waitForReply = true); + + /// \brief Reads the Synchronization Status register. + /// + /// \return The register's values. + SynchronizationStatusRegister readSynchronizationStatus(); + + /// \brief Writes to the Synchronization Status register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeSynchronizationStatus(SynchronizationStatusRegister &fields, bool waitForReply = true); + + /// \brief Writes to the Synchronization Status register. + /// + /// \param[in] syncInCount Value for the SyncInCount field. + /// \param[in] syncInTime Value for the SyncInTime field. + /// \param[in] syncOutCount Value for the SyncOutCount field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeSynchronizationStatus( + const uint32_t &syncInCount, + const uint32_t &syncInTime, + const uint32_t &syncOutCount, + bool waitForReply = true); + + /// \brief Reads the Filter Basic Control register. + /// + /// \return The register's values. + FilterBasicControlRegister readFilterBasicControl(); + + /// \brief Writes to the Filter Basic Control register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeFilterBasicControl(FilterBasicControlRegister &fields, bool waitForReply = true); + + /// \brief Writes to the Filter Basic Control register. + /// + /// \param[in] magMode Value for the MagMode field. + /// \param[in] extMagMode Value for the ExtMagMode field. + /// \param[in] extAccMode Value for the ExtAccMode field. + /// \param[in] extGyroMode Value for the ExtGyroMode field. + /// \param[in] gyroLimit Value for the GyroLimit field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeFilterBasicControl( + protocol::uart::MagneticMode magMode, + protocol::uart::ExternalSensorMode extMagMode, + protocol::uart::ExternalSensorMode extAccMode, + protocol::uart::ExternalSensorMode extGyroMode, + const math::vec3f &gyroLimit, + bool waitForReply = true); + + /// \brief Reads the VPE Basic Control register. + /// + /// \return The register's values. + VpeBasicControlRegister readVpeBasicControl(); + + /// \brief Writes to the VPE Basic Control register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeBasicControl(VpeBasicControlRegister &fields, bool waitForReply = true); + + /// \brief Writes to the VPE Basic Control register. + /// + /// \param[in] enable Value for the Enable field. + /// \param[in] headingMode Value for the HeadingMode field. + /// \param[in] filteringMode Value for the FilteringMode field. + /// \param[in] tuningMode Value for the TuningMode field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeBasicControl( + protocol::uart::VpeEnable enable, + protocol::uart::HeadingMode headingMode, + protocol::uart::VpeMode filteringMode, + protocol::uart::VpeMode tuningMode, + bool waitForReply = true); + + /// \brief Reads the VPE Magnetometer Basic Tuning register. + /// + /// \return The register's values. + VpeMagnetometerBasicTuningRegister readVpeMagnetometerBasicTuning(); + + /// \brief Writes to the VPE Magnetometer Basic Tuning register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeMagnetometerBasicTuning(VpeMagnetometerBasicTuningRegister &fields, bool waitForReply = true); + + /// \brief Writes to the VPE Magnetometer Basic Tuning register. + /// + /// \param[in] baseTuning Value for the BaseTuning field. + /// \param[in] adaptiveTuning Value for the AdaptiveTuning field. + /// \param[in] adaptiveFiltering Value for the AdaptiveFiltering field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeMagnetometerBasicTuning( + const math::vec3f &baseTuning, + const math::vec3f &adaptiveTuning, + const math::vec3f &adaptiveFiltering, + bool waitForReply = true); + + /// \brief Reads the VPE Magnetometer Advanced Tuning register. + /// + /// \return The register's values. + VpeMagnetometerAdvancedTuningRegister readVpeMagnetometerAdvancedTuning(); + + /// \brief Writes to the VPE Magnetometer Advanced Tuning register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeMagnetometerAdvancedTuning(VpeMagnetometerAdvancedTuningRegister &fields, bool waitForReply = true); + + /// \brief Writes to the VPE Magnetometer Advanced Tuning register. + /// + /// \param[in] minFiltering Value for the MinFiltering field. + /// \param[in] maxFiltering Value for the MaxFiltering field. + /// \param[in] maxAdaptRate Value for the MaxAdaptRate field. + /// \param[in] disturbanceWindow Value for the DisturbanceWindow field. + /// \param[in] maxTuning Value for the MaxTuning field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeMagnetometerAdvancedTuning( + const math::vec3f &minFiltering, + const math::vec3f &maxFiltering, + const float &maxAdaptRate, + const float &disturbanceWindow, + const float &maxTuning, + bool waitForReply = true); + + /// \brief Reads the VPE Accelerometer Basic Tuning register. + /// + /// \return The register's values. + VpeAccelerometerBasicTuningRegister readVpeAccelerometerBasicTuning(); + + /// \brief Writes to the VPE Accelerometer Basic Tuning register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeAccelerometerBasicTuning(VpeAccelerometerBasicTuningRegister &fields, bool waitForReply = true); + + /// \brief Writes to the VPE Accelerometer Basic Tuning register. + /// + /// \param[in] baseTuning Value for the BaseTuning field. + /// \param[in] adaptiveTuning Value for the AdaptiveTuning field. + /// \param[in] adaptiveFiltering Value for the AdaptiveFiltering field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeAccelerometerBasicTuning( + const math::vec3f &baseTuning, + const math::vec3f &adaptiveTuning, + const math::vec3f &adaptiveFiltering, + bool waitForReply = true); + + /// \brief Reads the VPE Accelerometer Advanced Tuning register. + /// + /// \return The register's values. + VpeAccelerometerAdvancedTuningRegister readVpeAccelerometerAdvancedTuning(); + + /// \brief Writes to the VPE Accelerometer Advanced Tuning register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeAccelerometerAdvancedTuning(VpeAccelerometerAdvancedTuningRegister &fields, bool waitForReply = true); + + /// \brief Writes to the VPE Accelerometer Advanced Tuning register. + /// + /// \param[in] minFiltering Value for the MinFiltering field. + /// \param[in] maxFiltering Value for the MaxFiltering field. + /// \param[in] maxAdaptRate Value for the MaxAdaptRate field. + /// \param[in] disturbanceWindow Value for the DisturbanceWindow field. + /// \param[in] maxTuning Value for the MaxTuning field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeAccelerometerAdvancedTuning( + const math::vec3f &minFiltering, + const math::vec3f &maxFiltering, + const float &maxAdaptRate, + const float &disturbanceWindow, + const float &maxTuning, + bool waitForReply = true); + + /// \brief Reads the VPE Gryo Basic Tuning register. + /// + /// \return The register's values. + VpeGryoBasicTuningRegister readVpeGryoBasicTuning(); + + /// \brief Writes to the VPE Gryo Basic Tuning register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeGryoBasicTuning(VpeGryoBasicTuningRegister &fields, bool waitForReply = true); + + /// \brief Writes to the VPE Gryo Basic Tuning register. + /// + /// \param[in] angularWalkVariance Value for the AngularWalkVariance field. + /// \param[in] baseTuning Value for the BaseTuning field. + /// \param[in] adaptiveTuning Value for the AdaptiveTuning field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVpeGryoBasicTuning( + const math::vec3f &angularWalkVariance, + const math::vec3f &baseTuning, + const math::vec3f &adaptiveTuning, + bool waitForReply = true); + + /// \brief Reads the Filter Startup Gyro Bias register. + /// + /// \return The register's values. + math::vec3f readFilterStartupGyroBias(); + + /// \brief Writes to the Filter Startup Gyro Bias register. + /// + /// \param[in] bias The register's Bias field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeFilterStartupGyroBias(const math::vec3f &bias, bool waitForReply = true); + + /// \brief Reads the Magnetometer Calibration Control register. + /// + /// \return The register's values. + MagnetometerCalibrationControlRegister readMagnetometerCalibrationControl(); + + /// \brief Writes to the Magnetometer Calibration Control register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeMagnetometerCalibrationControl(MagnetometerCalibrationControlRegister &fields, bool waitForReply = true); + + /// \brief Writes to the Magnetometer Calibration Control register. + /// + /// \param[in] hsiMode Value for the HSIMode field. + /// \param[in] hsiOutput Value for the HSIOutput field. + /// \param[in] convergeRate Value for the ConvergeRate field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeMagnetometerCalibrationControl( + protocol::uart::HsiMode hsiMode, + protocol::uart::HsiOutput hsiOutput, + const uint8_t &convergeRate, + bool waitForReply = true); + + /// \brief Reads the Calculated Magnetometer Calibration register. + /// + /// \return The register's values. + CalculatedMagnetometerCalibrationRegister readCalculatedMagnetometerCalibration(); + + /// \brief Reads the Indoor Heading Mode Control register. + /// + /// \return The register's values. + float readIndoorHeadingModeControl(); + + /// \brief Writes to the Indoor Heading Mode Control register. + /// + /// \param[in] maxRateError The register's Max Rate Error field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeIndoorHeadingModeControl(const float &maxRateError, bool waitForReply = true); + + /// \brief Reads the Velocity Compensation Measurement register. + /// + /// \return The register's values. + math::vec3f readVelocityCompensationMeasurement(); + + /// \brief Writes to the Velocity Compensation Measurement register. + /// + /// \param[in] velocity The register's Velocity field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVelocityCompensationMeasurement(const math::vec3f &velocity, bool waitForReply = true); + + /// \brief Reads the Velocity Compensation Control register. + /// + /// \return The register's values. + VelocityCompensationControlRegister readVelocityCompensationControl(); + + /// \brief Writes to the Velocity Compensation Control register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVelocityCompensationControl(VelocityCompensationControlRegister &fields, bool waitForReply = true); + + /// \brief Writes to the Velocity Compensation Control register. + /// + /// \param[in] mode Value for the Mode field. + /// \param[in] velocityTuning Value for the VelocityTuning field. + /// \param[in] rateTuning Value for the RateTuning field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeVelocityCompensationControl( + protocol::uart::VelocityCompensationMode mode, + const float &velocityTuning, + const float &rateTuning, + bool waitForReply = true); + + /// \brief Reads the Velocity Compensation Status register. + /// + /// \return The register's values. + VelocityCompensationStatusRegister readVelocityCompensationStatus(); + + /// \brief Reads the IMU Measurements register. + /// + /// \return The register's values. + ImuMeasurementsRegister readImuMeasurements(); + + /// \brief Reads the GPS Configuration register. + /// + /// \return The register's values. + GpsConfigurationRegister readGpsConfiguration(); + + /// \brief Writes to the GPS Configuration register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeGpsConfiguration(GpsConfigurationRegister &fields, bool waitForReply = true); + + /// \brief Writes to the GPS Configuration register. + /// + /// \param[in] mode Value for the Mode field. + /// \param[in] ppsSource Value for the PpsSource field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeGpsConfiguration( + protocol::uart::GpsMode mode, + protocol::uart::PpsSource ppsSource, + bool waitForReply = true); + + /// \brief Reads the GPS Antenna Offset register. + /// + /// \return The register's values. + math::vec3f readGpsAntennaOffset(); + + /// \brief Writes to the GPS Antenna Offset register. + /// + /// \param[in] position The register's Position field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeGpsAntennaOffset(const math::vec3f &position, bool waitForReply = true); + + /// \brief Reads the GPS Solution - LLA register. + /// + /// \return The register's values. + GpsSolutionLlaRegister readGpsSolutionLla(); + + /// \brief Reads the GPS Solution - ECEF register. + /// + /// \return The register's values. + GpsSolutionEcefRegister readGpsSolutionEcef(); + + /// \brief Reads the INS Solution - LLA register. + /// + /// \return The register's values. + InsSolutionLlaRegister readInsSolutionLla(); + + /// \brief Reads the INS Solution - ECEF register. + /// + /// \return The register's values. + InsSolutionEcefRegister readInsSolutionEcef(); + + /// \brief Reads the INS Advanced Configuration register. + /// + /// \return The register's values. + InsAdvancedConfigurationRegister readInsAdvancedConfiguration(); + + /// \brief Writes to the INS Advanced Configuration register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeInsAdvancedConfiguration(InsAdvancedConfigurationRegister &fields, bool waitForReply = true); + + /// \brief Writes to the INS Advanced Configuration register. + /// + /// \param[in] useMag Value for the UseMag field. + /// \param[in] usePres Value for the UsePres field. + /// \param[in] posAtt Value for the PosAtt field. + /// \param[in] velAtt Value for the VelAtt field. + /// \param[in] velBias Value for the VelBias field. + /// \param[in] useFoam Value for the UseFoam field. + /// \param[in] gpsCovType Value for the GPSCovType field. + /// \param[in] velCount Value for the VelCount field. + /// \param[in] velInit Value for the VelInit field. + /// \param[in] moveOrigin Value for the MoveOrigin field. + /// \param[in] gpsTimeout Value for the GPSTimeout field. + /// \param[in] deltaLimitPos Value for the DeltaLimitPos field. + /// \param[in] deltaLimitVel Value for the DeltaLimitVel field. + /// \param[in] minPosUncertainty Value for the MinPosUncertainty field. + /// \param[in] minVelUncertainty Value for the MinVelUncertainty field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeInsAdvancedConfiguration( + const uint8_t &useMag, + const uint8_t &usePres, + const uint8_t &posAtt, + const uint8_t &velAtt, + const uint8_t &velBias, + protocol::uart::FoamInit useFoam, + const uint8_t &gpsCovType, + const uint8_t &velCount, + const float &velInit, + const float &moveOrigin, + const float &gpsTimeout, + const float &deltaLimitPos, + const float &deltaLimitVel, + const float &minPosUncertainty, + const float &minVelUncertainty, + bool waitForReply = true); + + /// \brief Reads the INS State - LLA register. + /// + /// \return The register's values. + InsStateLlaRegister readInsStateLla(); + + /// \brief Reads the INS State - ECEF register. + /// + /// \return The register's values. + InsStateEcefRegister readInsStateEcef(); + + /// \brief Reads the Startup Filter Bias Estimate register. + /// + /// \return The register's values. + StartupFilterBiasEstimateRegister readStartupFilterBiasEstimate(); + + /// \brief Writes to the Startup Filter Bias Estimate register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeStartupFilterBiasEstimate(StartupFilterBiasEstimateRegister &fields, bool waitForReply = true); + + /// \brief Writes to the Startup Filter Bias Estimate register. + /// + /// \param[in] gyroBias Value for the GyroBias field. + /// \param[in] accelBias Value for the AccelBias field. + /// \param[in] pressureBias Value for the PressureBias field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeStartupFilterBiasEstimate( + const math::vec3f &gyroBias, + const math::vec3f &accelBias, + const float &pressureBias, + bool waitForReply = true); + + /// \brief Reads the Delta Theta and Delta Velocity register. + /// + /// \return The register's values. + DeltaThetaAndDeltaVelocityRegister readDeltaThetaAndDeltaVelocity(); + + /// \brief Reads the Delta Theta and Delta Velocity Configuration register. + /// + /// \return The register's values. + DeltaThetaAndDeltaVelocityConfigurationRegister readDeltaThetaAndDeltaVelocityConfiguration(); + + /// \brief Writes to the Delta Theta and Delta Velocity Configuration register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeDeltaThetaAndDeltaVelocityConfiguration(DeltaThetaAndDeltaVelocityConfigurationRegister &fields, bool waitForReply = true); + + /// \brief Writes to the Delta Theta and Delta Velocity Configuration register. + /// + /// \param[in] integrationFrame Value for the IntegrationFrame field. + /// \param[in] gyroCompensation Value for the GyroCompensation field. + /// \param[in] accelCompensation Value for the AccelCompensation field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeDeltaThetaAndDeltaVelocityConfiguration( + protocol::uart::IntegrationFrame integrationFrame, + protocol::uart::CompensationMode gyroCompensation, + protocol::uart::CompensationMode accelCompensation, + bool waitForReply = true); + + /// \brief Reads the Reference Vector Configuration register. + /// + /// \return The register's values. + ReferenceVectorConfigurationRegister readReferenceVectorConfiguration(); + + /// \brief Writes to the Reference Vector Configuration register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeReferenceVectorConfiguration(ReferenceVectorConfigurationRegister &fields, bool waitForReply = true); + + /// \brief Writes to the Reference Vector Configuration register. + /// + /// \param[in] useMagModel Value for the UseMagModel field. + /// \param[in] useGravityModel Value for the UseGravityModel field. + /// \param[in] recalcThreshold Value for the RecalcThreshold field. + /// \param[in] year Value for the Year field. + /// \param[in] position Value for the Position field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeReferenceVectorConfiguration( + const uint8_t &useMagModel, + const uint8_t &useGravityModel, + const uint32_t &recalcThreshold, + const float &year, + const math::vec3d &position, + bool waitForReply = true); + + /// \brief Reads the Gyro Compensation register. + /// + /// \return The register's values. + GyroCompensationRegister readGyroCompensation(); + + /// \brief Writes to the Gyro Compensation register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeGyroCompensation(GyroCompensationRegister &fields, bool waitForReply = true); + + /// \brief Writes to the Gyro Compensation register. + /// + /// \param[in] c Value for the C field. + /// \param[in] b Value for the B field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeGyroCompensation( + const math::mat3f &c, + const math::vec3f &b, + bool waitForReply = true); + + /// \brief Reads the IMU Filtering Configuration register. + /// + /// \return The register's values. + ImuFilteringConfigurationRegister readImuFilteringConfiguration(); + + /// \brief Writes to the IMU Filtering Configuration register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeImuFilteringConfiguration(ImuFilteringConfigurationRegister &fields, bool waitForReply = true); + + /// \brief Writes to the IMU Filtering Configuration register. + /// + /// \param[in] magWindowSize Value for the MagWindowSize field. + /// \param[in] accelWindowSize Value for the AccelWindowSize field. + /// \param[in] gyroWindowSize Value for the GyroWindowSize field. + /// \param[in] tempWindowSize Value for the TempWindowSize field. + /// \param[in] presWindowSize Value for the PresWindowSize field. + /// \param[in] magFilterMode Value for the MagFilterMode field. + /// \param[in] accelFilterMode Value for the AccelFilterMode field. + /// \param[in] gyroFilterMode Value for the GyroFilterMode field. + /// \param[in] tempFilterMode Value for the TempFilterMode field. + /// \param[in] presFilterMode Value for the PresFilterMode field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeImuFilteringConfiguration( + const uint16_t &magWindowSize, + const uint16_t &accelWindowSize, + const uint16_t &gyroWindowSize, + const uint16_t &tempWindowSize, + const uint16_t &presWindowSize, + protocol::uart::FilterMode magFilterMode, + protocol::uart::FilterMode accelFilterMode, + protocol::uart::FilterMode gyroFilterMode, + protocol::uart::FilterMode tempFilterMode, + protocol::uart::FilterMode presFilterMode, + bool waitForReply = true); + + /// \brief Reads the GPS Compass Baseline register. + /// + /// \return The register's values. + GpsCompassBaselineRegister readGpsCompassBaseline(); + + /// \brief Writes to the GPS Compass Baseline register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeGpsCompassBaseline(GpsCompassBaselineRegister &fields, bool waitForReply = true); + + /// \brief Writes to the GPS Compass Baseline register. + /// + /// \param[in] position Value for the Position field. + /// \param[in] uncertainty Value for the Uncertainty field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeGpsCompassBaseline( + const math::vec3f &position, + const math::vec3f &uncertainty, + bool waitForReply = true); + + /// \brief Reads the GPS Compass Estimated Baseline register. + /// + /// \return The register's values. + GpsCompassEstimatedBaselineRegister readGpsCompassEstimatedBaseline(); + + /// \brief Reads the IMU Rate Configuration register. + /// + /// \return The register's values. + ImuRateConfigurationRegister readImuRateConfiguration(); + + /// \brief Writes to the IMU Rate Configuration register. + /// + /// \param[in] fields The register's fields. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeImuRateConfiguration(ImuRateConfigurationRegister &fields, bool waitForReply = true); + + /// \brief Writes to the IMU Rate Configuration register. + /// + /// \param[in] imuRate Value for the imuRate field. + /// \param[in] navDivisor Value for the NavDivisor field. + /// \param[in] filterTargetRate Value for the filterTargetRate field. + /// \param[in] filterMinRate Value for the filterMinRate field. + /// \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + void writeImuRateConfiguration( + const uint16_t &imuRate, + const uint16_t &navDivisor, + const float &filterTargetRate, + const float &filterMinRate, + bool waitForReply = true); + + /// \brief Reads the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register. + /// + /// \return The register's values. + YawPitchRollTrueBodyAccelerationAndAngularRatesRegister readYawPitchRollTrueBodyAccelerationAndAngularRates(); + + /// \brief Reads the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register. + /// + /// \return The register's values. + YawPitchRollTrueInertialAccelerationAndAngularRatesRegister readYawPitchRollTrueInertialAccelerationAndAngularRates(); + + /// \} + + #ifdef PYTHON_WRAPPERS + + #endif + + #if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 + + void stopRequest(); + bool threadStopped(); + void unregisterListners(); + void shutdownRequest(); + void goRequest(); + + #endif + +private: + struct Impl; + Impl *_pi; + +}; + +} +} + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/util/boostpython.h b/ext/vnproglib-1.1.0.115/include/vn/util/boostpython.h new file mode 100755 index 0000000..8d1b682 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/util/boostpython.h @@ -0,0 +1,25 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#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 diff --git a/ext/vnproglib-1.1.0.115/include/vn/util/compiler.h b/ext/vnproglib-1.1.0.115/include/vn/util/compiler.h new file mode 100755 index 0000000..08d1fce --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/util/compiler.h @@ -0,0 +1,81 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#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 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 d({ &ez->_persistentData, &nd }); +// #else +// vector 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 diff --git a/ext/vnproglib-1.1.0.115/include/vn/util/dllvalidator.h b/ext/vnproglib-1.1.0.115/include/vn/util/dllvalidator.h new file mode 100755 index 0000000..e24d003 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/util/dllvalidator.h @@ -0,0 +1,68 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifndef _DLLVALIDATOR_H_ +#define _DLLVALIDATOR_H_ + +#if defined WIN32 + +#include +#include + +#if defined(_MSC_VER) + // Disable a bunch of warnings from 3rd party library. + #pragma warning(push) + #pragma warning(disable:4091) + #pragma warning(disable:4251) + #pragma warning(disable:4275) +#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& dllNamesOut); + + void getMissingDllNames(std::vector& missingDllNamesOut); + + bool validate(); + +private: + struct DllValidatorVisitor : public PeLib::PeFileVisitor + { + //template + //void dumpImportDirectory(PeLib::PeFile& pef, std::vector& dllNamesOut); + + virtual void callback(PeLib::PeFile32 &file); + + virtual void callback(PeLib::PeFile64 &file); + + std::vector mRequiredDlls; + }; + + DllValidator(); + + bool mIsInitialized; + bool mIsValid; + DllValidatorVisitor mVisitor; + PeLib::PeFile* mPeFile; + std::string mFileName; + std::string mWorkingDirectory; + std::vector mMissingDlls; +}; + +#endif + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/util/export.h b/ext/vnproglib-1.1.0.115/include/vn/util/export.h new file mode 100755 index 0000000..6604adf --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/util/export.h @@ -0,0 +1,26 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#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 diff --git a/ext/vnproglib-1.1.0.115/include/vn/util/memoryport.h b/ext/vnproglib-1.1.0.115/include/vn/util/memoryport.h new file mode 100755 index 0000000..86be51f --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/util/memoryport.h @@ -0,0 +1,106 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +/// \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 + +#if defined(_MSC_VER) + #pragma warning(pop) +#endif + +#include "vn/int.h" +#include "vn/xplat/port.h" +#include "vn/util/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 diff --git a/ext/vnproglib-1.1.0.115/include/vn/util/nocopy.h b/ext/vnproglib-1.1.0.115/include/vn/util/nocopy.h new file mode 100755 index 0000000..2a59190 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/util/nocopy.h @@ -0,0 +1,47 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +/// \file +/// {COMMON_HEADER} +#ifndef _VNUTIL_NOCOPY_H_ +#define _VNUTIL_NOCOPY_H_ + +#include "vn/util/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 diff --git a/ext/vnproglib-1.1.0.115/include/vn/utilities.h b/ext/vnproglib-1.1.0.115/include/vn/utilities.h new file mode 100755 index 0000000..6db6523 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/utilities.h @@ -0,0 +1,130 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +/// \file +/// {COMMON_HEADER} +#ifndef _VN_UTILITIES_H_ +#define _VN_UTILITIES_H_ + +#include +#include + +#include "vn/int.h" +#include "vn/util/export.h" + +/// \brief Defines for the specific version of the VectorNav library. +#define VNAPI_MAJOR 1 +#define VNAPI_MINOR 1 +#define VNAPI_PATCH 0 +#define VNAPI_REVISION 115 + +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& missingDlls); + +#endif + +} + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/xplat/criticalsection.h b/ext/vnproglib-1.1.0.115/include/vn/xplat/criticalsection.h new file mode 100755 index 0000000..e71a177 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/xplat/criticalsection.h @@ -0,0 +1,54 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +/// \file +/// {COMMON_HEADER} +/// +/// \section DESCRIPTION +/// This header file provides the class CriticalSection. +#ifndef _VNXPLAT_CRITICALSECTION_H_ +#define _VNXPLAT_CRITICALSECTION_H_ + +#include "vn/util/nocopy.h" +#include "vn/util/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 diff --git a/ext/vnproglib-1.1.0.115/include/vn/xplat/event.h b/ext/vnproglib-1.1.0.115/include/vn/xplat/event.h new file mode 100755 index 0000000..f4fa638 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/xplat/event.h @@ -0,0 +1,75 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifndef _VNXPLAT_EVENT_H_ +#define _VNXPLAT_EVENT_H_ + +#include "vn/util/nocopy.h" +#include "vn/int.h" +#include "vn/util/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 diff --git a/ext/vnproglib-1.1.0.115/include/vn/xplat/port.h b/ext/vnproglib-1.1.0.115/include/vn/xplat/port.h new file mode 100755 index 0000000..66cdfd4 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/xplat/port.h @@ -0,0 +1,84 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifndef _VN_XPLAT_PORT_H_ +#define _VN_XPLAT_PORT_H_ + +#include + +#include "vn/util/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 true if the serial port is open; otherwise false. + virtual bool isOpen() = 0; + + /// \brief Writes out data to the simple port. + /// + /// \param[in] data The data array to write out. + /// \param[in] length The length of the data array to write out. + virtual void write(const char data[], size_t length) = 0; + + /// \brief Allows reading data from the simple port. + /// + /// \param[out] dataBuffer The data buffer to write the read data bytes to. + /// \param[in] numOfBytesToRead The number of bytes to attempt reading from + /// the simple port. + /// \param[out] numOfBytesActuallyRead The number of bytes actually read + /// from the simple port. + virtual void read(char dataBuffer[], size_t numOfBytesToRead, size_t &numOfBytesActuallyRead) = 0; + + /// \brief Registers a callback method for notification when new data is + /// received on the port. + /// + /// \param[in] userData Pointer to user data, which will be provided to the + /// callback method. + /// \param[in] handler The callback method. + virtual void registerDataReceivedHandler(void* userData, DataReceivedHandler handler) = 0; + + /// \brief Unregisters the registered callback method. + virtual void unregisterDataReceivedHandler() = 0; + + #if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 + + virtual void stopThread(){} + virtual bool threadStopped(){ return false; } + virtual void resumeThread(){} + + #endif + +}; + +} +} + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/xplat/serialport.h b/ext/vnproglib-1.1.0.115/include/vn/xplat/serialport.h new file mode 100755 index 0000000..693e5b4 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/xplat/serialport.h @@ -0,0 +1,216 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifndef _VN_XPLAT_SERIALPORT_H_ +#define _VN_XPLAT_SERIALPORT_H_ + +#if _MSC_VER && _WIN32 + #pragma comment(lib, "setupapi.lib") +#endif + +#include +#include +#include + +#if PYTHON + #include "vn/util/boostpython.h" +#endif + +#include "vn/int.h" +#include "vn/xplat/port.h" +#include "vn/util/nocopy.h" +#include "vn/util/export.h" + +namespace vn { +namespace xplat { + +#if TO_BE_REVIEWED +//template +class Event +{ +public: + + #if PYTHON + //EventT& operator+=(int test) + //void operator+=(int test) + void operator+=(PyObject* handler) + { + _pycallables.push_back(handler); + handler->ob_refcnt++; + //_handlers.push_back(test); + + //return *this; + } + + #endif + + void fire() + { + #if PYTHON + for (auto h : _pycallables) + boost::python::call(h); + #endif + + //vector pRawData(readBuffer, readBuffer + numOfBytesRead); + + //python::AcquireGIL scopedLock; + + //boost::python::call(pi->_rawDataReceivedHandlerPython, pRawData, pi->_dataRunningIndex); + + //for (auto h : _handlers) +// std::cout << h << std::endl; + } + +private: + #if PYTHON + std::list _pycallables; + #endif + std::list _handlers; + /*def __init__(self) : + self.__handlers = [] + + def __iadd__(self, handler) : + self.__handlers.append(handler) + return self + + def __isub__(self, handler) : + self.__handlers.remove(handler) + return self + + def fire(self, *args, **keywargs) : + for handler in self.__handlers : + handler(*args, **keywargs) + + def clearObjectHandlers(self, inObject) : + for theHandler in self.__handlers : + if theHandler.im_self == inObject : + self -= theHandler*/ +}; + +#endif + +/// \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 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 Changes the connected baudrate of the port. + /// + /// \param[in] br The baudrate to change the port to. + void changeBaudrate(uint32_t br); + + /// \brief Returns the stop bit configuration. + /// + /// \return The current stop bit configuration. + StopBits stopBits(); + + /// \brief Sets the stop bit configuration. + /// + /// \param[in] stopBits The stop bit configuration. + void setStopBits(StopBits stopBits); + + /// \brief Indicates if the platforms supports event notifications. + + /// \brief Returns the number of dropped sections of received data. + /// + /// \return The number of sections of dropped data sections. Note this is + /// not indicative of the total number of dropped bytes. + size_t NumberOfReceiveDataDroppedSections(); + + /// \brief With regard to optimizing COM ports provided by FTDI drivers, this + /// method will check if the COM port has been optimized. + /// + /// \param[in] portName The COM port name to check. + /// \return true if the COM port is optimized; otherwise false. + static bool determineIfPortIsOptimized(std::string portName); + + /// \brief This will perform optimization of FTDI USB serial ports. + /// + /// If calling this method on Windows, the process must have administrator + /// privileges to write settings to the registry. Otherwise an + /// + /// \param[in] portName The FTDI USB Serial Port to optimize. + static void optimizePort(std::string portName); + + #if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 + + virtual void stopThread(); + virtual void resumeThread(); + virtual bool threadStopped(); + + #endif + + // Private Members //////////////////////////////////////////////////////// + +private: + + // Contains internal data, mainly stuff that is required for cross-platform + // support. + struct Impl; + Impl *_pi; + +}; + +} +} + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/xplat/signal.h b/ext/vnproglib-1.1.0.115/include/vn/xplat/signal.h new file mode 100755 index 0000000..a303f29 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/xplat/signal.h @@ -0,0 +1,93 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#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 Ctrl-C. + CTRL_C + }; + + /// \brief Typedef for a function that can handle signal notifications. + /// + /// \param[in] signal The signal type received. + /// \return Indicates if the signal was handled or not. + typedef bool (*HandleSignalFunc)(Signal signal); + + /// \brief Allows for other objects to listen for signal events. + class Observer + { + public: + virtual ~Observer() { } + + /// \brief If an observer is registered via + /// \ref RegisterSignalObserver, whenever a signal is received, + /// this method will be called to alert the observer. + /// + /// \param[in] signal The signal that was raised. + /// \return Indicates if the signal was handled or not. + virtual bool XSignalHandleSingle(SignalType signal) = 0; + + protected: + Observer() { } + }; + + // Public Methods ///////////////////////////////////////////////////////// + +public: + + /// \brief Allows registering to receive notifications of system signals. + /// + /// \param[in] handleFunc Function to call when signals are received. + static void RegisterForSignalNotifications( + HandleSignalFunc handleFunc); + + /// \brief Allows unregistering from receiving signal notifications. + /// + /// \param[in] handleFunc The function to unregister. + static void UnregisterForSignalNotifications( + HandleSignalFunc handleFunc); + + /// \brief Allows registering an observer for notification when a signal is + /// received. + /// + /// \param[in] observer The observer to register. + static void RegisterSignalObserver( + Observer* observer); + + /// \brief Allows unregistering of an observer from being notified when a + /// signal is received. + /// + /// \param[in] observer The observer to unregister. + static void UnregisterSignalObserver( + Observer* observer); + + // Private Members //////////////////////////////////////////////////////// + +private: + + // Contains internal data, mainly stuff that is required for cross-platform + // support. + struct Impl; + +}; + +} +} + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/xplat/thread.h b/ext/vnproglib-1.1.0.115/include/vn/xplat/thread.h new file mode 100755 index 0000000..ac1c8a3 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/xplat/thread.h @@ -0,0 +1,101 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +/// \file +/// {COMMON_HEADER} +/// +/// \section DESCRIPTION +/// This header file provides the class Thread. +#ifndef _VNXPLAT_THREAD_H_ +#define _VNXPLAT_THREAD_H_ + +#include "vn/int.h" +#include "vn/util/export.h" +#include "vn/util/nocopy.h" + +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 Thread 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 Thread object representing the newly started thread. + /// User must delete the returned pointer when finished. + static Thread* startNew(ThreadStartRoutine startRoutine, void* routineData); + + /// \brief Starts the thread. + /// + /// \param[in] routineData Pointer to the routine data which the new thread + /// have access to. + void start(void* routineData); + + /// \brief Blocks the calling thread until this thread finishes. + void join(); + + /// \brief Causes the thread to sleep the specified number of seconds. + /// + /// \param[in] numOfSecsToSleep The number of seconds to sleep. + static void sleepSec(uint32_t numOfSecsToSleep); + + /// \brief Causes the thread to sleep the specified number of milliseconds. + /// + /// \param[in] numOfMsToSleep The number of milliseconds to sleep. + static void sleepMs(uint32_t numOfMsToSleep); + + /// \brief Causes the thread to sleep the specified number of microseconds. + /// + /// \param[in] numOfUsToSleep The number of microseconds to sleep. + static void sleepUs(uint32_t numOfUsToSleep); + + /// \brief Causes the thread to sleep the specified number of nanoseconds. + /// + /// \param[in] numOfNsToSleep The number of nanoseconds to sleep. + static void sleepNs(uint32_t numOfNsToSleep); + + // Private Members //////////////////////////////////////////////////////// + +private: + + // Contains internal data, mainly stuff that is required for cross-platform + // support. + struct Impl; + Impl *_pimpl; + +}; + +} +} + +#endif diff --git a/ext/vnproglib-1.1.0.115/include/vn/xplat/time.h b/ext/vnproglib-1.1.0.115/include/vn/xplat/time.h new file mode 100755 index 0000000..24e3ca6 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/include/vn/xplat/time.h @@ -0,0 +1,61 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifndef _VNXPLAT_TIME_H_ +#define _VNXPLAT_TIME_H_ + +#include "vn/int.h" +#include "vn/util/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. + int64_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 diff --git a/ext/vnproglib-1.1.0.115/src/vn/benchmark.cpp b/ext/vnproglib-1.1.0.115/src/vn/benchmark.cpp new file mode 100755 index 0000000..61d38a3 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/benchmark.cpp @@ -0,0 +1,28 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifdef _win32 +#include +#include + +#endif + +#include +#include + +#include "hayai_main.hpp" + +int main(int argc, char* argv[]) +{ + // Set up the main runner. + ::hayai::MainRunner runner; + + // Parse the arguments. + int result = runner.ParseArgs(argc, argv); + if (result) + { + return result; + } + + // Execute based on the selected mode. + return runner.Run(); +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/data/error_detection.cpp b/ext/vnproglib-1.1.0.115/src/vn/data/error_detection.cpp new file mode 100755 index 0000000..11c34af --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/data/error_detection.cpp @@ -0,0 +1,47 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/data/error_detection.h" + +#include +using namespace std; + +namespace vn { +namespace data { +namespace integrity { + +uint8_t Checksum8::compute(char const data[], size_t length) +{ + if (length > 1000) + cout << "HAPPENING!" << endl; + + uint8_t xorVal = 0; + + for (size_t i = 0; i < length; i++) + { + xorVal ^= data[i]; + } + + return xorVal; +} + +uint16_t Crc16::compute(char const data[], size_t length) +{ + uint32_t i; + uint16_t crc = 0; + + for (i = 0; i < length; i++) + { + crc = static_cast((crc >> 8) | (crc << 8)); + + crc ^= static_cast(data[i]); + crc ^= static_cast(static_cast(crc & 0xFF) >> 4); + crc ^= static_cast((crc << 8) << 4); + crc ^= static_cast(((crc & 0xFF) << 4) << 1); + } + + return crc; +} + +} +} +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/event.cpp b/ext/vnproglib-1.1.0.115/src/vn/event.cpp new file mode 100755 index 0000000..a3f134b --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/event.cpp @@ -0,0 +1,77 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/event.h" + +/*#if _WIN32 + #include + #include + #include + #include + #if _UNICODE + #else + #include + #endif +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + #include + #include + #include + #include + #include + #include + #include + #include +#else + #error "Unknown System" +#endif + +#if __linux__ + #include +#elif __APPLE__ + #include +#endif + +#include +#include + +#include "vn/xplat/thread.h" +#include "vn/xplat/criticalsection.h" +#include "vn/exceptions.h" +#include + +using namespace std;*/ + +#if PYTHON +namespace bp = boost::python; +#endif + +namespace vn { + +#if OLD +void Event::operator+=(PyObject* handler) +{ +} + +void Event::add(PyObject* handler) +{ + _pycallables.push_back(handler); + handler->ob_refcnt++; +} + +//void Event::fire() +//{ +// for (auto h : _pycallables) +// boost::python::call(h); +//} + +bp::tuple Event::fire(bp::tuple args, bp::dict kwargs) +{ + // TEMP + for (auto h : _pycallables) + boost::python::call(h); + + return bp::make_tuple(); +} + +#endif + +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/main.cpp b/ext/vnproglib-1.1.0.115/src/vn/main.cpp new file mode 100755 index 0000000..a19c2aa --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/main.cpp @@ -0,0 +1,22 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#ifdef _win32 +#include +#include + +#endif + +#include +#include + +#include "gtest/gtest.h" + +int main(int argc, char* argv[]) +{ + int i = 0; + char* c = "Testing1234"; + + testing::InitGoogleTest(&i, &c); + + return RUN_ALL_TESTS(); +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/math/attitude.cpp b/ext/vnproglib-1.1.0.115/src/vn/math/attitude.cpp new file mode 100755 index 0000000..61abc01 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/math/attitude.cpp @@ -0,0 +1,122 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/math/attitude.h" + +#include "vn/exceptions.h" +#include "vn/math/conversions.h" + +using namespace std; + +namespace vn { +namespace math { + +AttitudeF AttitudeF::noRotation() +{ + vec4f q(0, 0, 0, 1); + + return AttitudeF(ATT_Quat, &q); +} + +AttitudeF AttitudeF::fromQuat(vec4f quat) +{ + return AttitudeF(ATT_Quat, &quat); +} + +AttitudeF AttitudeF::fromYprInDegs(vec3f yprInDegs) +{ + return AttitudeF(ATT_YprDegs, &yprInDegs); +} + +AttitudeF AttitudeF::fromYprInRads(vec3f yprInRads) +{ + return AttitudeF(ATT_YprRads, &yprInRads); +} + +AttitudeF AttitudeF::fromDcm(mat3f dcm) +{ + return AttitudeF(ATT_Dcm, &dcm); +} + +AttitudeF::AttitudeF(AttitudeType type, void* attitude) : + _underlyingType(type) +{ + size_t numOfBytesToCopy = sizeof(vec3f); + + if (_underlyingType == ATT_Quat) + numOfBytesToCopy = sizeof(vec4f); + else if (_underlyingType == ATT_Dcm) + numOfBytesToCopy = sizeof(mat3f); + + copy(static_cast(attitude), static_cast(attitude) + numOfBytesToCopy, _data); +} + +vec3f AttitudeF::yprInDegs() +{ + switch (_underlyingType) + { + case ATT_YprDegs: + return *static_cast(static_cast(_data)); + case ATT_YprRads: + return rad2deg(*static_cast(static_cast(_data))); + case ATT_Quat: + return quat2YprInDegs(*static_cast(static_cast(_data))); + case ATT_Dcm: + return dcm2YprInDegs(*static_cast(static_cast(_data))); + default: + throw not_implemented(); + } +} + +vec3f AttitudeF::yprInRads() +{ + switch (_underlyingType) + { + case ATT_YprRads: + return *static_cast(static_cast(_data)); + case ATT_YprDegs: + return deg2rad(*static_cast(static_cast(_data))); + case ATT_Quat: + return quat2YprInRads(*static_cast(static_cast(_data))); + case ATT_Dcm: + return dcm2YprInRads(*static_cast(static_cast(_data))); + default: + throw not_implemented(); + } +} + +vec4f AttitudeF::quat() +{ + switch (_underlyingType) + { + case ATT_Quat: + return *static_cast(static_cast(_data)); + case ATT_YprDegs: + return yprInDegs2Quat(*static_cast(static_cast(_data))); + case ATT_YprRads: + return yprInRads2Quat(*static_cast(static_cast(_data))); + case ATT_Dcm: + return dcm2quat(*static_cast(static_cast(_data))); + default: + throw not_implemented(); + } +} + +mat3f AttitudeF::dcm() +{ + switch (_underlyingType) + { + case ATT_Dcm: + return *static_cast(static_cast(_data)); + case ATT_YprDegs: + return yprInDegs2Dcm(*static_cast(static_cast(_data))); + case ATT_YprRads: + return yprInRads2Dcm(*static_cast(static_cast(_data))); + case ATT_Quat: + return quat2dcm(*static_cast(static_cast(_data))); + default: + throw not_implemented(); + } +} + +} +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/math/conversions.cpp b/ext/vnproglib-1.1.0.115/src/vn/math/conversions.cpp new file mode 100755 index 0000000..4d07185 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/math/conversions.cpp @@ -0,0 +1,295 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +/// {COMMON_HEADER} +#include "vn/math/conversions.h" + +#include "vn/math/consts.h" + +namespace vn { +namespace math { + +float rad2deg(float angleInRads) +{ + return angleInRads * 180.0f / PIf; +} + +double rad2deg(double angleInRads) +{ + return angleInRads * 180.0 / PId; +} + +float deg2rad(float angleInDegs) +{ + return angleInDegs * PIf / 180.0f; +} + +double deg2rad(double angleInDegs) +{ + return angleInDegs * PId / 180.0f; +} + +float celsius2fahren(float tempInCelsius) +{ + return (tempInCelsius * 9.0f) / 5.0f + 32.0f; +} + +double celsius2fahren(double tempInCelsius) +{ + return (tempInCelsius * 9.0) / 5.0 + 32.0; +} + +float fahren2celsius(float tempInFahren) +{ + return (tempInFahren - 32.0f) * 5.0f / 9.0f; +} + +double fahren2celsius(double tempInFahren) +{ + return (tempInFahren - 32.0) * 5.0 / 9.0; +} + +float celsius2kelvin(float tempInCelsius) +{ + return tempInCelsius + 273.15f; +} + +double celsius2kelvin(double tempInCelsius) +{ + return tempInCelsius + 273.15; +} + +float kelvin2celsius(float tempInKelvin) +{ + return tempInKelvin - 273.15f; +} + +double kelvin2celsius(double tempInKelvin) +{ + return tempInKelvin - 273.15; +} + +float fahren2kelvin(float tempInFahren) +{ + return fahren2celsius(tempInFahren) + 273.15f; +} + +double fahren2kelvin(double tempInFahren) +{ + return fahren2celsius(tempInFahren) + 273.15; +} + +float kelvin2fahren(float tempInKelvin) +{ + return celsius2fahren(tempInKelvin - 273.15f); +} + +double kelvin2fahren(double tempInKelvin) +{ + return celsius2fahren(tempInKelvin - 273.15); +} + +vec4f yprInDegs2Quat(vec3f yprInDegs) +{ + return yprInRads2Quat(deg2rad(yprInDegs)); +} + +vec4f yprInRads2Quat(vec3f ypr) +{ + float c1 = cos(ypr.x / 2.0f); + float s1 = sin(ypr.x / 2.0f); + float c2 = cos(ypr.y / 2.0f); + float s2 = sin(ypr.y / 2.0f); + float c3 = cos(ypr.z / 2.0f); + float s3 = sin(ypr.z / 2.0f); + + return vec4f( + c1*c2*s3 - s1*s2*c3, + c1*s2*c3 + s1*c2*s3, + s1*c2*c3 - c1*s2*s3, + c1*c2*c3 + s1*s2*s3); +} + +mat3f yprInDegs2Dcm(vec3f yprInDegs) +{ + return yprInRads2Dcm(deg2rad(yprInDegs)); +} + +mat3f yprInRads2Dcm(vec3f yprInRads) +{ + float st1 = sin(yprInRads.x); + float ct1 = cos(yprInRads.x); + float st2 = sin(yprInRads.y); + float ct2 = cos(yprInRads.y); + float st3 = sin(yprInRads.z); + float ct3 = cos(yprInRads.z); + + #if PROPOSED_NEW_FORMUALA + // PL-175 - YPR to DCM conversion error + return mat3f( + ct2 * ct1, + -ct2 * st1, + st2, + st3 * st2 * ct1 + ct3 * st1, + -st3 * st2 * st1 + ct3 * ct1, + -st3 * ct2, + -ct3 * st2 * ct1 + st3 * st1, + ct3 * st2 * st1 + st3 * ct1, + ct3 * ct2); + #else + return mat3f( + ct2 * ct1, + ct2 * st1, + -st2, + st3 * st2 * ct1 - ct3 * st1, + st3 * st2 * st1 + ct3 * ct1, + st3 * ct2, + ct3 * st2 * ct1 + st3 * st1, + ct3 * st2 * st1 - st3 * ct1, + ct3 * ct2); + #endif +} + +vec3f quat2YprInDegs(vec4f quat) +{ + return rad2deg(quat2YprInRads(quat)); +} + +vec3f quat2YprInRads(vec4f quat) +{ + float q1 = quat.x; + float q2 = quat.y; + float q3 = quat.z; + float q0 = quat.w; + + return vec3f( + atan2(2.0f * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3), + asin(-2.0f * (q1 * q3 - q0 * q2)), + atan2(2.0f * (q2 * q3 + q0 * q1), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3)); +} + +mat3f quat2dcm(vec4f quat) +{ + float q1 = quat.x; + float q2 = quat.y; + float q3 = quat.z; + float q0 = quat.w; + + return mat3f( + q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3, + 2.f * (q1 * q2 + q0 * q3), + 2.f * (q1 * q3 - q0 * q2), + 2.f * (q1 * q2 - q0 * q3), + q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3, + 2.f * (q2 * q3 + q0 * q1), + 2.f * (q1 * q3 + q0 * q2), + 2.f * (q2 * q3 - q0 * q1), + q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); +} + +vec3f dcm2YprInDegs(mat3f dcm) +{ + return rad2deg(dcm2YprInRads(dcm)); +} + +vec3f dcm2YprInRads(mat3f dcm) +{ + return vec3f( + atan2(dcm.e01, dcm.e00), + asin(-dcm.e02), + atan2(dcm.e12, dcm.e22)); +} + +vec4f dcm2quat(mat3f dcm) +{ + float tr = dcm.e00 + dcm.e11 + dcm.e22; + + float b2[] = { + (1.f + tr) / 4.f, + (1.f + 2.f * dcm.e00 - tr) / 4.f, + (1.f + 2.f * dcm.e11 - tr) / 4.f, + (1.f + 2.f * dcm.e22 - tr) / 4.f }; + + float maxNum = b2[0]; + size_t maxIndex = 0; + for (size_t i = 1; i < sizeof(b2); i++) + { + if (b2[i] > maxNum) + { + maxNum = b2[i]; + maxIndex = i; + } + } + + vec4f q; + + switch (maxIndex) + { + case 0: + q.w = sqrt(b2[0]); + q.x = (dcm.e12 - dcm.e21) / 4.f / q.w; + q.y = (dcm.e20 - dcm.e02) / 4.f / q.w; + q.z = (dcm.e01 - dcm.e10) / 4.f / q.w; + break; + + case 1: + q.x = sqrt(b2[1]); + q.w = (dcm.e12 - dcm.e21) / 4.f / q.x; + if (q.w < 0) + { + q.x = -q.x; + q.w = -q.w; + } + q.y = (dcm.e01 + dcm.e10) / 4.f / q.x; + q.z = (dcm.e20 + dcm.e02) / 4.f / q.x; + break; + + case 2: + q.y = sqrt(b2[2]); + q.w = (dcm.e20 - dcm.e02) / 4.f / q.y; + if (q.w < 0) + { + q.y = -q.y; + q.w = -q.w; + } + q.x = (dcm.e01 + dcm.e10) / 4.f / q.y; + q.z = (dcm.e12 + dcm.e21) / 4.f / q.y; + break; + + case 3: + q.z = sqrt(b2[3]); + q.w = (dcm.e01 - dcm.e10) / 4.f / q.z; + if (q.w < 0) + { + q.z = -q.z; + q.w = -q.w; + } + q.x = (dcm.e20 + dcm.e02) / 4.f / q.z; + q.y = (dcm.e12 + dcm.e21) / 4.f / q.z; + break; + } + + return q; +} + +float course_over_ground(float velNedX, float velNedY) +{ + return atan2(velNedY, velNedX); +} + +float course_over_ground(vec3f velNed) +{ + return course_over_ground(velNed.x, velNed.y); +} + +float speed_over_ground(float velNedX, float velNedY) +{ + return vec2f(velNedX, velNedY).mag(); +} + +float speed_over_ground(vec3f velNed) +{ + return speed_over_ground(velNed.x, velNed.y); +} + +} +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/math/position.cpp b/ext/vnproglib-1.1.0.115/src/vn/math/position.cpp new file mode 100755 index 0000000..e357828 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/math/position.cpp @@ -0,0 +1,26 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/math/position.h" + +using namespace std; + +namespace vn { +namespace math { + +PositionD::PositionD(PositionType type, vec3d pos) : + _underlyingType(type), + _data(pos) +{ } + +PositionD PositionD::fromLla(vec3d lla) +{ + return PositionD(POS_LLA, lla); +} + +PositionD PositionD::fromEcef(vec3d ecef) +{ + return PositionD(POS_ECEF, ecef); +} + +} +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/math/python.cpp b/ext/vnproglib-1.1.0.115/src/vn/math/python.cpp new file mode 100755 index 0000000..b02bf71 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/math/python.cpp @@ -0,0 +1,1604 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/util/boostpython.h" + +#include "vn/math/vector.h" +#include "vn/math/matrix.h" +#include "vn/math/conversions.h" +#include "vn/math/attitude.h" + +using namespace boost::python; +using namespace vn::math; + +BOOST_PYTHON_MODULE(_math) +{ + docstring_options local_docstring_options(true, true, false); + class_("vec2f", "2-component vector using floats.") + .def(init()) + .def(init()) + .def_readwrite("x", &vec2f::x) + .def_readwrite("y", &vec2f::y) + .def("dim", &vec2f::dim) + .add_static_property("zero", &vec2f::zero) + .add_static_property("one", &vec2f::one) + .add_static_property("unit_x", &vec2f::unitX) + .add_static_property("unit_y", &vec2f::unitY) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(-self) + .def( "mag", &vec2f::mag) + .def("norm", &vec2f::norm); + + class_("vec2d", "2-component vector using doubles.") + .def(init()) + .def(init()) + .def_readwrite("x", &vec2d::x) + .def_readwrite("y", &vec2d::y) + .def("dim", &vec2d::dim) + .add_static_property("zero", &vec2d::zero) + .add_static_property("one", &vec2d::one) + .add_static_property("unit_x", &vec2d::unitX) + .add_static_property("unit_y", &vec2d::unitY) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(-self) + .def( "mag", &vec2d::mag) + .def("norm", &vec2d::norm); + + class_("vec2ld", "2-component vector using long doubles.") + .def(init()) + .def(init()) + .def_readwrite("x", &vec2ld::x) + .def_readwrite("y", &vec2ld::y) + .def("dim", &vec2ld::dim) + .add_static_property("zero", &vec2ld::zero) + .add_static_property("one", &vec2ld::one) + .add_static_property("unit_x", &vec2ld::unitX) + .add_static_property("unit_y", &vec2ld::unitY) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(-self) + .def( "mag", &vec2ld::mag) + .def("norm", &vec2ld::norm); + + class_("vec2i32", "2-component vector using int32_ts.") + .def(init()) + .def(init()) + .def_readwrite("x", &vec2i32::x) + .def_readwrite("y", &vec2i32::y) + .def("dim", &vec2i32::dim) + .add_static_property("zero", &vec2i32::zero) + .add_static_property("one", &vec2i32::one) + .add_static_property("unit_x", &vec2i32::unitX) + .add_static_property("unit_y", &vec2i32::unitY) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(-self) + .def( "mag", &vec2i32::mag) + .def("norm", &vec2i32::norm); + + class_("vec2u32", "2-component vector using uint32_ts.") + .def(init()) + .def(init()) + .def_readwrite("x", &vec2u32::x) + .def_readwrite("y", &vec2u32::y) + .def("dim", &vec2u32::dim) + .add_static_property("zero", &vec2u32::zero) + .add_static_property("one", &vec2u32::one) + .add_static_property("unit_x", &vec2u32::unitX) + .add_static_property("unit_y", &vec2u32::unitY) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(-self) + .def( "mag", &vec2u32::mag) + .def("norm", &vec2u32::norm); + + class_("vec3f", "3-component vector using floats.") + .def(init()) + .def(init()) + .def_readwrite("x", &vec3f::x) + .def_readwrite("y", &vec3f::y) + .def_readwrite("z", &vec3f::z) + .def_readwrite("r", &vec3f::r) + .def_readwrite("g", &vec3f::g) + .def_readwrite("b", &vec3f::b) + .def("dim", &vec3f::dim) + .add_static_property("zero", &vec3f::zero) + .add_static_property("one", &vec3f::one) + .add_static_property("unit_x", &vec3f::unitX) + .add_static_property("unit_y", &vec3f::unitY) + .add_static_property("unit_z", &vec3f::unitZ) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(-self) + .def( "mag", &vec3f::mag) + .def("norm", &vec3f::norm); + + class_("vec3d", "3-component vector using doubles.") + .def(init()) + .def(init()) + .def_readwrite("x", &vec3d::x) + .def_readwrite("y", &vec3d::y) + .def_readwrite("z", &vec3d::z) + .def_readwrite("r", &vec3d::r) + .def_readwrite("g", &vec3d::g) + .def_readwrite("b", &vec3d::b) + .def("dim", &vec3d::dim) + .add_static_property("zero", &vec3d::zero) + .add_static_property("one", &vec3d::one) + .add_static_property("unit_x", &vec3d::unitX) + .add_static_property("unit_y", &vec3d::unitY) + .add_static_property("unit_z", &vec3d::unitZ) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(-self) + .def( "mag", &vec3d::mag) + .def("norm", &vec3d::norm); + + class_("vec3ld", "3-component vector using long doubles.") + .def(init()) + .def(init()) + .def_readwrite("x", &vec3ld::x) + .def_readwrite("y", &vec3ld::y) + .def_readwrite("z", &vec3ld::z) + .def_readwrite("r", &vec3ld::r) + .def_readwrite("g", &vec3ld::g) + .def_readwrite("b", &vec3ld::b) + .def("dim", &vec3ld::dim) + .add_static_property("zero", &vec3ld::zero) + .add_static_property("one", &vec3ld::one) + .add_static_property("unit_x", &vec3ld::unitX) + .add_static_property("unit_y", &vec3ld::unitY) + .add_static_property("unit_z", &vec3ld::unitZ) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(-self) + .def( "mag", &vec3ld::mag) + .def("norm", &vec3ld::norm); + + class_("vec3i32", "3-component vector using int32_ts.") + .def(init()) + .def(init()) + .def_readwrite("x", &vec3i32::x) + .def_readwrite("y", &vec3i32::y) + .def_readwrite("z", &vec3i32::z) + .def_readwrite("r", &vec3i32::r) + .def_readwrite("g", &vec3i32::g) + .def_readwrite("b", &vec3i32::b) + .def("dim", &vec3i32::dim) + .add_static_property("zero", &vec3i32::zero) + .add_static_property("one", &vec3i32::one) + .add_static_property("unit_x", &vec3i32::unitX) + .add_static_property("unit_y", &vec3i32::unitY) + .add_static_property("unit_z", &vec3i32::unitZ) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(-self) + .def( "mag", &vec3i32::mag) + .def("norm", &vec3i32::norm); + + class_("vec3u32", "3-component vector using uint32_ts.") + .def(init()) + .def(init()) + .def_readwrite("x", &vec3u32::x) + .def_readwrite("y", &vec3u32::y) + .def_readwrite("z", &vec3u32::z) + .def_readwrite("r", &vec3u32::r) + .def_readwrite("g", &vec3u32::g) + .def_readwrite("b", &vec3u32::b) + .def("dim", &vec3u32::dim) + .add_static_property("zero", &vec3u32::zero) + .add_static_property("one", &vec3u32::one) + .add_static_property("unit_x", &vec3u32::unitX) + .add_static_property("unit_y", &vec3u32::unitY) + .add_static_property("unit_z", &vec3u32::unitZ) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(-self) + .def( "mag", &vec3u32::mag) + .def("norm", &vec3u32::norm); + + class_("vec4f", "4-component vector using floats.") + .def(init()) + .def(init()) + .def_readwrite("x", &vec4f::x) + .def_readwrite("y", &vec4f::y) + .def_readwrite("z", &vec4f::z) + .def_readwrite("w", &vec4f::w) + .def_readwrite("r", &vec4f::r) + .def_readwrite("g", &vec4f::g) + .def_readwrite("b", &vec4f::b) + .def_readwrite("a", &vec4f::a) + .def("dim", &vec4f::dim) + .add_static_property("zero", &vec4f::zero) + .add_static_property("one", &vec4f::one) + .add_static_property("unit_x", &vec4f::unitX) + .add_static_property("unit_y", &vec4f::unitY) + .add_static_property("unit_z", &vec4f::unitZ) + .add_static_property("unit_w", &vec4f::unitW) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(-self) + .def( "mag", &vec4f::mag) + .def("norm", &vec4f::norm); + + class_("vec4d", "4-component vector using doubles.") + .def(init()) + .def(init()) + .def_readwrite("x", &vec4d::x) + .def_readwrite("y", &vec4d::y) + .def_readwrite("z", &vec4d::z) + .def_readwrite("w", &vec4d::w) + .def_readwrite("r", &vec4d::r) + .def_readwrite("g", &vec4d::g) + .def_readwrite("b", &vec4d::b) + .def_readwrite("a", &vec4d::a) + .def("dim", &vec4d::dim) + .add_static_property("zero", &vec4d::zero) + .add_static_property("one", &vec4d::one) + .add_static_property("unit_x", &vec4d::unitX) + .add_static_property("unit_y", &vec4d::unitY) + .add_static_property("unit_z", &vec4d::unitZ) + .add_static_property("unit_w", &vec4d::unitW) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(-self) + .def( "mag", &vec4d::mag) + .def("norm", &vec4d::norm); + + class_("vec4ld", "4-component vector using long doubles.") + .def(init()) + .def(init()) + .def_readwrite("x", &vec4ld::x) + .def_readwrite("y", &vec4ld::y) + .def_readwrite("z", &vec4ld::z) + .def_readwrite("w", &vec4ld::w) + .def_readwrite("r", &vec4ld::r) + .def_readwrite("g", &vec4ld::g) + .def_readwrite("b", &vec4ld::b) + .def_readwrite("a", &vec4ld::a) + .def("dim", &vec4ld::dim) + .add_static_property("zero", &vec4ld::zero) + .add_static_property("one", &vec4ld::one) + .add_static_property("unit_x", &vec4ld::unitX) + .add_static_property("unit_y", &vec4ld::unitY) + .add_static_property("unit_z", &vec4ld::unitZ) + .add_static_property("unit_w", &vec4ld::unitW) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(-self) + .def( "mag", &vec4ld::mag) + .def("norm", &vec4ld::norm); + + class_("vec4i32", "4-component vector using int32_ts.") + .def(init()) + .def(init()) + .def_readwrite("x", &vec4i32::x) + .def_readwrite("y", &vec4i32::y) + .def_readwrite("z", &vec4i32::z) + .def_readwrite("w", &vec4i32::w) + .def_readwrite("r", &vec4i32::r) + .def_readwrite("g", &vec4i32::g) + .def_readwrite("b", &vec4i32::b) + .def_readwrite("a", &vec4i32::a) + .def("dim", &vec4i32::dim) + .add_static_property("zero", &vec4i32::zero) + .add_static_property("one", &vec4i32::one) + .add_static_property("unit_x", &vec4i32::unitX) + .add_static_property("unit_y", &vec4i32::unitY) + .add_static_property("unit_z", &vec4i32::unitZ) + .add_static_property("unit_w", &vec4i32::unitW) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(-self) + .def( "mag", &vec4i32::mag) + .def("norm", &vec4i32::norm); + + class_("vec4u32", "4-component vector using uint32_ts.") + .def(init()) + .def(init()) + .def_readwrite("x", &vec4u32::x) + .def_readwrite("y", &vec4u32::y) + .def_readwrite("z", &vec4u32::z) + .def_readwrite("w", &vec4u32::w) + .def_readwrite("r", &vec4u32::r) + .def_readwrite("g", &vec4u32::g) + .def_readwrite("b", &vec4u32::b) + .def_readwrite("a", &vec4u32::a) + .def("dim", &vec4u32::dim) + .add_static_property("zero", &vec4u32::zero) + .add_static_property("one", &vec4u32::one) + .add_static_property("unit_x", &vec4u32::unitX) + .add_static_property("unit_y", &vec4u32::unitY) + .add_static_property("unit_z", &vec4u32::unitZ) + .add_static_property("unit_w", &vec4u32::unitW) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(self * other()) + .def(other() * self) + .def(self / other()) + .def(-self) + .def( "mag", &vec4u32::mag) + .def("norm", &vec4u32::norm); + class_("mat2f", "2-component matrix using floats.") + .def(init()) + .def(init()) + .def_readwrite("e00", &mat2f::e00) + .def_readwrite("e01", &mat2f::e01) + .def_readwrite("e10", &mat2f::e10) + .def_readwrite("e11", &mat2f::e11) + .def("dim_row", &mat2f::dimRow) + .def("dim_col", &mat2f::dimCol) + .add_static_property("zero", &mat2f::zero) + .add_static_property("one", &mat2f::one) + .add_static_property("identity", &mat2f::identity) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * self) + .def(self += self) + .def(self -= self) + .def(self *= self) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2f::copy) + .def(-self); + + class_("mat2d", "2-component matrix using doubles.") + .def(init()) + .def(init()) + .def_readwrite("e00", &mat2d::e00) + .def_readwrite("e01", &mat2d::e01) + .def_readwrite("e10", &mat2d::e10) + .def_readwrite("e11", &mat2d::e11) + .def("dim_row", &mat2d::dimRow) + .def("dim_col", &mat2d::dimCol) + .add_static_property("zero", &mat2d::zero) + .add_static_property("one", &mat2d::one) + .add_static_property("identity", &mat2d::identity) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * self) + .def(self += self) + .def(self -= self) + .def(self *= self) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2d::copy) + .def(-self); + + class_("mat2ld", "2-component matrix using long doubles.") + .def(init()) + .def(init()) + .def_readwrite("e00", &mat2ld::e00) + .def_readwrite("e01", &mat2ld::e01) + .def_readwrite("e10", &mat2ld::e10) + .def_readwrite("e11", &mat2ld::e11) + .def("dim_row", &mat2ld::dimRow) + .def("dim_col", &mat2ld::dimCol) + .add_static_property("zero", &mat2ld::zero) + .add_static_property("one", &mat2ld::one) + .add_static_property("identity", &mat2ld::identity) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * self) + .def(self += self) + .def(self -= self) + .def(self *= self) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat2ld::copy) + .def(-self); + + class_("mat3f", "3-component matrix using floats.") + .def(init()) + .def(init()) + .def_readwrite("e00", &mat3f::e00) + .def_readwrite("e01", &mat3f::e01) + .def_readwrite("e02", &mat3f::e02) + .def_readwrite("e10", &mat3f::e10) + .def_readwrite("e11", &mat3f::e11) + .def_readwrite("e12", &mat3f::e12) + .def_readwrite("e20", &mat3f::e20) + .def_readwrite("e21", &mat3f::e21) + .def_readwrite("e22", &mat3f::e22) + .def("dim_row", &mat3f::dimRow) + .def("dim_col", &mat3f::dimCol) + .add_static_property("zero", &mat3f::zero) + .add_static_property("one", &mat3f::one) + .add_static_property("identity", &mat3f::identity) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * self) + .def(self += self) + .def(self -= self) + .def(self *= self) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3f::copy) + .def(-self); + + class_("mat3d", "3-component matrix using doubles.") + .def(init()) + .def(init()) + .def_readwrite("e00", &mat3d::e00) + .def_readwrite("e01", &mat3d::e01) + .def_readwrite("e02", &mat3d::e02) + .def_readwrite("e10", &mat3d::e10) + .def_readwrite("e11", &mat3d::e11) + .def_readwrite("e12", &mat3d::e12) + .def_readwrite("e20", &mat3d::e20) + .def_readwrite("e21", &mat3d::e21) + .def_readwrite("e22", &mat3d::e22) + .def("dim_row", &mat3d::dimRow) + .def("dim_col", &mat3d::dimCol) + .add_static_property("zero", &mat3d::zero) + .add_static_property("one", &mat3d::one) + .add_static_property("identity", &mat3d::identity) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * self) + .def(self += self) + .def(self -= self) + .def(self *= self) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3d::copy) + .def(-self); + + class_("mat3ld", "3-component matrix using long doubles.") + .def(init()) + .def(init()) + .def_readwrite("e00", &mat3ld::e00) + .def_readwrite("e01", &mat3ld::e01) + .def_readwrite("e02", &mat3ld::e02) + .def_readwrite("e10", &mat3ld::e10) + .def_readwrite("e11", &mat3ld::e11) + .def_readwrite("e12", &mat3ld::e12) + .def_readwrite("e20", &mat3ld::e20) + .def_readwrite("e21", &mat3ld::e21) + .def_readwrite("e22", &mat3ld::e22) + .def("dim_row", &mat3ld::dimRow) + .def("dim_col", &mat3ld::dimCol) + .add_static_property("zero", &mat3ld::zero) + .add_static_property("one", &mat3ld::one) + .add_static_property("identity", &mat3ld::identity) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * self) + .def(self += self) + .def(self -= self) + .def(self *= self) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat3ld::copy) + .def(-self); + + class_("mat4f", "4-component matrix using floats.") + .def(init()) + .def(init()) + .def_readwrite("e00", &mat4f::e00) + .def_readwrite("e01", &mat4f::e01) + .def_readwrite("e02", &mat4f::e02) + .def_readwrite("e03", &mat4f::e03) + .def_readwrite("e10", &mat4f::e10) + .def_readwrite("e11", &mat4f::e11) + .def_readwrite("e12", &mat4f::e12) + .def_readwrite("e13", &mat4f::e13) + .def_readwrite("e20", &mat4f::e20) + .def_readwrite("e21", &mat4f::e21) + .def_readwrite("e22", &mat4f::e22) + .def_readwrite("e23", &mat4f::e23) + .def_readwrite("e30", &mat4f::e30) + .def_readwrite("e31", &mat4f::e31) + .def_readwrite("e32", &mat4f::e32) + .def_readwrite("e33", &mat4f::e33) + .def("dim_row", &mat4f::dimRow) + .def("dim_col", &mat4f::dimCol) + .add_static_property("zero", &mat4f::zero) + .add_static_property("one", &mat4f::one) + .add_static_property("identity", &mat4f::identity) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * self) + .def(self += self) + .def(self -= self) + .def(self *= self) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4f::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4f::copy) + .def(-self); + + class_("mat4d", "4-component matrix using doubles.") + .def(init()) + .def(init()) + .def_readwrite("e00", &mat4d::e00) + .def_readwrite("e01", &mat4d::e01) + .def_readwrite("e02", &mat4d::e02) + .def_readwrite("e03", &mat4d::e03) + .def_readwrite("e10", &mat4d::e10) + .def_readwrite("e11", &mat4d::e11) + .def_readwrite("e12", &mat4d::e12) + .def_readwrite("e13", &mat4d::e13) + .def_readwrite("e20", &mat4d::e20) + .def_readwrite("e21", &mat4d::e21) + .def_readwrite("e22", &mat4d::e22) + .def_readwrite("e23", &mat4d::e23) + .def_readwrite("e30", &mat4d::e30) + .def_readwrite("e31", &mat4d::e31) + .def_readwrite("e32", &mat4d::e32) + .def_readwrite("e33", &mat4d::e33) + .def("dim_row", &mat4d::dimRow) + .def("dim_col", &mat4d::dimCol) + .add_static_property("zero", &mat4d::zero) + .add_static_property("one", &mat4d::one) + .add_static_property("identity", &mat4d::identity) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * self) + .def(self += self) + .def(self -= self) + .def(self *= self) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4d::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4d::copy) + .def(-self); + + class_("mat4ld", "4-component matrix using long doubles.") + .def(init()) + .def(init()) + .def_readwrite("e00", &mat4ld::e00) + .def_readwrite("e01", &mat4ld::e01) + .def_readwrite("e02", &mat4ld::e02) + .def_readwrite("e03", &mat4ld::e03) + .def_readwrite("e10", &mat4ld::e10) + .def_readwrite("e11", &mat4ld::e11) + .def_readwrite("e12", &mat4ld::e12) + .def_readwrite("e13", &mat4ld::e13) + .def_readwrite("e20", &mat4ld::e20) + .def_readwrite("e21", &mat4ld::e21) + .def_readwrite("e22", &mat4ld::e22) + .def_readwrite("e23", &mat4ld::e23) + .def_readwrite("e30", &mat4ld::e30) + .def_readwrite("e31", &mat4ld::e31) + .def_readwrite("e32", &mat4ld::e32) + .def_readwrite("e33", &mat4ld::e33) + .def("dim_row", &mat4ld::dimRow) + .def("dim_col", &mat4ld::dimCol) + .add_static_property("zero", &mat4ld::zero) + .add_static_property("one", &mat4ld::one) + .add_static_property("identity", &mat4ld::identity) + .def(self_ns::str(self_ns::self)) + .def(self + self) + .def(self - self) + .def(self * self) + .def(self += self) + .def(self -= self) + .def(self *= self) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4ld::copy) + .def(self * other()) + .def(self *= other()) + .def(other() * self) + .def(self / other()) + .def(self /= other()) + .def("copy", &mat4ld::copy) + .def(-self); + + class_("AttitudeF", no_init) + .def("no_rotation", &AttitudeF::noRotation).staticmethod("no_rotation") + .def("from_quat", &AttitudeF::fromQuat).staticmethod("from_quat") + .def("from_ypr_in_degs", &AttitudeF::fromYprInDegs).staticmethod("from_ypr_in_degs") + .def("from_ypr_in_rads", &AttitudeF::fromYprInRads).staticmethod("from_ypr_in_rads") + .def("from_dcm", &AttitudeF::fromDcm).staticmethod("from_dcm") + .def_readonly("ypr_in_degs", &AttitudeF::yprInDegs) + .def_readonly("ypr_in_rads", &AttitudeF::yprInRads) + .def_readonly("quat", &AttitudeF::quat) + .def_readonly("dcm", &AttitudeF::dcm) + ; + + def("deg2rad", static_cast(deg2rad<3>)); + + /// \brief Converts an angle in degrees to radians. + /// + /// \param[in] angleInDegs The angle in degrees. + /// \return The angle converted to radians. + //float 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 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 + //vec deg2rad(vec anglesInDegs) + + //class_("yprf", "Yaw, pitch, roll using floats.", no_init) + // .def("from_degs", static_cast(&yprf::fromDegs)).staticmethod("from_degs") + // .def("to_vec3", &yprf::toVec3) + // ; + //.def("parse", static_cast(&CompositeData::parse)).staticmethod("parse") + +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/protocol/uart/packet.cpp b/ext/vnproglib-1.1.0.115/src/vn/protocol/uart/packet.cpp new file mode 100755 index 0000000..3b44b12 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/protocol/uart/packet.cpp @@ -0,0 +1,3966 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/protocol/uart/packet.h" + +// TODO : Remove all C style coding where possible +#include + +// TODO : Make this more compiler compatible incase +// the user's compiler is not C++11 compliant +#include +#include +#include +#include +#include + +#include + +#include "vn/utilities.h" +#include "vn/data/error_detection.h" +#include "vn/util/compiler.h" + +#if PYTHON + #include "boost/python.hpp" + #include "vn/python/util.h" + namespace bp = boost::python; +#endif + +#define NEXT result = getNextData(_data, parseIndex); \ + if (result == NULL) \ + return; + +#define ATOFF static_cast(std::atof(result)) +#define ATOFD std::atof(result) +#define ATOU32 static_cast(std::atoi(result)) +#define ATOU16X ((uint16_t) std::strtol(result, NULL, 16)) +#define ATOU16 static_cast(std::atoi(result)) +#define ATOU8 static_cast(std::atoi(result)) + +using namespace std; +using namespace vn::math; +//using namespace vn::xplat; +using namespace vn::data::integrity; + +namespace vn { +namespace protocol { +namespace uart { + +char* vnstrtok(char* str, size_t& startIndex); + +const unsigned char Packet::BinaryGroupLengths[sizeof(uint8_t)*8][sizeof(uint16_t)*8] = { + { 8, 8, 8, 12, 16, 12, 24, 12, 12, 24, 20, 28, 2, 4, 8, 0 }, // Group 1 + { 8, 8, 8, 2, 8, 8, 8, 4, 0, 0, 0, 0, 0, 0, 0, 0 }, // Group 2 + { 2, 12, 12, 12, 4, 4, 16, 12, 12, 12, 12, 2, 40, 0, 0, 0 }, // Group 3 + { 8, 8, 2, 1, 1, 24, 24, 12, 12, 12, 4, 4, 32, 0, 0, 0 }, // Group 4 + { 2, 12, 16, 36, 12, 12, 12, 12, 12, 12, 28, 24, 0, 0, 0, 0 }, // Group 5 + { 2, 24, 24, 12, 12, 12, 12, 12, 12, 4, 4, 68, 64, 0, 0, 0 }, // Group 6 + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Invalid group + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 } // Invalid group +}; + +Packet::Packet() : + _isPacketDataMine(false), + _length(0) +{ +} + +Packet::Packet(char const* packet, size_t length) : + _isPacketDataMine(true), + _length(length), + _data(new char[length]), + _curExtractLoc(0) +{ + std::memcpy(_data, packet, length); +} + +Packet::Packet(string packet) : + _isPacketDataMine(true), + _length(packet.size()), + _data(new char[packet.size()]), + _curExtractLoc(0) +{ + std::memcpy(_data, packet.c_str(), packet.size()); +} + +Packet::Packet(Packet const& toCopy) : + _isPacketDataMine(true), + _length(toCopy._length), + _data(new char[toCopy._length]), + _curExtractLoc(0) +{ + std::memcpy(_data, toCopy._data, toCopy._length); +} + +Packet::~Packet() +{ + if (_isPacketDataMine) + delete [] _data; +} + +Packet& Packet::operator=(Packet const& from) +{ + if (_isPacketDataMine) + delete [] _data; + + _isPacketDataMine = true; + _data = new char[from._length]; + _length = from._length; + _curExtractLoc = from._curExtractLoc; + + std::memcpy(_data, from._data, from._length); + + return *this; +} + +string Packet::datastr() +{ + return string(_data, _length); +} + +Packet::Type Packet::type() +{ + if (_length < 1) + throw invalid_operation("Packet does not contain any data."); + + // #TODO : Added this for the if check below due to type checking + // Sometimes the 0xFA is compared as a signed char and + // sometimes as an unsigned char. + const unsigned char binary_indicator = 0XFA; + // Since this is not a pointer use the functional cast for C++ rather than the C style cast + const unsigned char data_zero = static_cast(_data[0]); //unsigned char(_data[0]); + + if (_data[0] == '$') + return TYPE_ASCII; + //if (_data[0] == binary_indicator) + //if (_data[0] == 0xFA) + if (data_zero == binary_indicator) + return TYPE_BINARY; + + return TYPE_UNKNOWN; +} + +bool Packet::isValid() +{ + if (_length == 0) + return false; + + if (type() == TYPE_ASCII) + { + // First determine if this packet does not have a checksum or CRC. + if (_data[_length - 3] == 'X' && _data[_length - 4] == 'X') + return true; + + // First determine if this packet has an 8-bit checksum or a 16-bit CRC. + if (_data[_length - 5] == '*') + { + // Appears we have an 8-bit checksum packet. + uint8_t expectedChecksum = toUint8FromHexStr(_data + _length - 4); + + uint8_t computedChecksum = Checksum8::compute(_data + 1, _length - 6); + + return expectedChecksum == computedChecksum; + } + else if (_data[_length - 7] == '*') + { + // Appears we have a 16-bit CRC packet. + uint16_t packetCrc = to_uint16_from_hexstr(_data + _length - 6); + + uint16_t computedCrc = Crc16::compute(_data + 1, _length - 8); + + return packetCrc == computedCrc; + } + else + { + // Don't know what we have. + return false; + } + } + else if (type() == TYPE_BINARY) + { + uint16_t computedCrc = Crc16::compute(_data + 1, _length - 1); + + return computedCrc == 0; + } + else + { + throw not_implemented(); + } +} + +bool Packet::isError() +{ + return std::strncmp(_data + 3, "ERR", 3) == 0; +} + +bool Packet::isResponse() +{ + if (std::strncmp(_data + 3, "WRG", 3) == 0) + return true; + if (std::strncmp(_data + 3, "RRG", 3) == 0) + return true; + if (std::strncmp(_data + 3, "WNV", 3) == 0) + return true; + if (std::strncmp(_data + 3, "RFS", 3) == 0) + return true; + if (std::strncmp(_data + 3, "RST", 3) == 0) + return true; + if (std::strncmp(_data + 3, "FWU", 3) == 0) + return true; + if (std::strncmp(_data + 3, "CMD", 3) == 0) + return true; + if (std::strncmp(_data + 3, "ASY", 3) == 0) + return true; + if (std::strncmp(_data + 3, "TAR", 3) == 0) + return true; + if (std::strncmp(_data + 3, "KMD", 3) == 0) + return true; + if (std::strncmp(_data + 3, "KAD", 3) == 0) + return true; + if (std::strncmp(_data + 3, "SGB", 3) == 0) + return true; + + return false; +} + +bool Packet::isAsciiAsync() +{ + // Pointer to the unique asynchronous data type identifier. + char* pAT = _data + 3; + + if (strncmp(pAT, "YPR", 3) == 0) + return true; + if (strncmp(pAT, "QTN", 3) == 0) + return true; + #ifdef INTERNAL + if (strncmp(pAT, "QTM", 3) == 0) + return true; + if (strncmp(pAT, "QTA", 3) == 0) + return true; + if (strncmp(pAT, "QTR", 3) == 0) + return true; + if (strncmp(pAT, "QMA", 3) == 0) + return true; + if (strncmp(pAT, "QAR", 3) == 0) + return true; + #endif + if (strncmp(pAT, "QMR", 3) == 0) + return true; + #ifdef INTERNAL + if (strncmp(pAT, "DCM", 3) == 0) + return true; + #endif + if (strncmp(pAT, "MAG", 3) == 0) + return true; + if (strncmp(pAT, "ACC", 3) == 0) + return true; + if (strncmp(pAT, "GYR", 3) == 0) + return true; + if (strncmp(pAT, "MAR", 3) == 0) + return true; + if (strncmp(pAT, "YMR", 3) == 0) + return true; + #ifdef INTERNAL + if (strncmp(pAT, "YCM", 3) == 0) + return true; + #endif + if (strncmp(pAT, "YBA", 3) == 0) + return true; + if (strncmp(pAT, "YIA", 3) == 0) + return true; + #ifdef INTERNAL + if (strncmp(pAT, "ICM", 3) == 0) + return true; + #endif + if (strncmp(pAT, "IMU", 3) == 0) + return true; + if (strncmp(pAT, "GPS", 3) == 0) + return true; + if (strncmp(pAT, "GPE", 3) == 0) + return true; + if (strncmp(pAT, "INS", 3) == 0) + return true; + if (strncmp(pAT, "INE", 3) == 0) + return true; + if (strncmp(pAT, "ISL", 3) == 0) + return true; + if (strncmp(pAT, "ISE", 3) == 0) + return true; + if (strncmp(pAT, "DTV", 3) == 0) + return true; + #ifdef INTERNAL + if (strncmp(pAT, "RAW", 3) == 0) + return true; + if (strncmp(pAT, "CMV", 3) == 0) + return true; + if (strncmp(pAT, "STV", 3) == 0) + return true; + if (strncmp(pAT, "COV", 3) == 0) + return true; + #endif + else + return false; +} + +AsciiAsync Packet::determineAsciiAsyncType() +{ + // Pointer to the unique asynchronous data type identifier. + char* pAT = _data + 3; + + if (strncmp(pAT, "YPR", 3) == 0) + return VNYPR; + if (strncmp(pAT, "QTN", 3) == 0) + return VNQTN; + #ifdef INTERNAL + if (strncmp(pAT, "QTM", 3) == 0) + return VNQTM; + if (strncmp(pAT, "QTA", 3) == 0) + return VNQTA; + if (strncmp(pAT, "QTR", 3) == 0) + return VNQTR; + if (strncmp(pAT, "QMA", 3) == 0) + return VNQMA; + if (strncmp(pAT, "QAR", 3) == 0) + return VNQAR; + #endif + if (strncmp(pAT, "QMR", 3) == 0) + return VNQMR; + #ifdef INTERNAL + if (strncmp(pAT, "DCM", 3) == 0) + return VNDCM; + #endif + if (strncmp(pAT, "MAG", 3) == 0) + return VNMAG; + if (strncmp(pAT, "ACC", 3) == 0) + return VNACC; + if (strncmp(pAT, "GYR", 3) == 0) + return VNGYR; + if (strncmp(pAT, "MAR", 3) == 0) + return VNMAR; + if (strncmp(pAT, "YMR", 3) == 0) + return VNYMR; + #ifdef INTERNAL + if (strncmp(pAT, "YCM", 3) == 0) + return VNYCM; + #endif + if (strncmp(pAT, "YBA", 3) == 0) + return VNYBA; + if (strncmp(pAT, "YIA", 3) == 0) + return VNYIA; + #ifdef INTERNAL + if (strncmp(pAT, "ICM", 3) == 0) + return VNICM; + #endif + if (strncmp(pAT, "IMU", 3) == 0) + return VNIMU; + if (strncmp(pAT, "GPS", 3) == 0) + return VNGPS; + if (strncmp(pAT, "GPE", 3) == 0) + return VNGPE; + if (strncmp(pAT, "INS", 3) == 0) + return VNINS; + if (strncmp(pAT, "INE", 3) == 0) + return VNINE; + if (strncmp(pAT, "ISL", 3) == 0) + return VNISL; + if (strncmp(pAT, "ISE", 3) == 0) + return VNISE; + if (strncmp(pAT, "DTV", 3) == 0) + return VNDTV; + #ifdef INTERNAL + if (strncmp(pAT, "RAW", 3) == 0) + return VNRAW; + if (strncmp(pAT, "CMV", 3) == 0) + return VNCMV; + if (strncmp(pAT, "STV", 3) == 0) + return VNSTV; + if (strncmp(pAT, "COV", 3) == 0) + return VNCOV; + #endif + else + throw unknown_error(); +} + +bool Packet::isCompatible(CommonGroup commonGroup, TimeGroup timeGroup, ImuGroup imuGroup, GpsGroup gpsGroup, AttitudeGroup attitudeGroup, InsGroup insGroup) +{ + // First make sure the appropriate groups are specified. + uint8_t groups = _data[1]; + char *curField = _data + 2; + + if (commonGroup) + { + if (*reinterpret_cast(curField) != commonGroup) + // Not the expected collection of field data types. + return false; + + curField += 2; + } + else if (groups & 0x01) + { + // There is unexpected Common Group data. + return false; + } + + if (timeGroup) + { + if (*reinterpret_cast(curField) != timeGroup) + // Not the expected collection of field data types. + return false; + + curField += 2; + } + else if (groups & 0x02) + { + // There is unexpected Time Group data. + return false; + } + + if (imuGroup) + { + if (*reinterpret_cast(curField) != imuGroup) + // Not the expected collection of field data types. + return false; + + curField += 2; + } + else if (groups & 0x04) + { + // There is unexpected IMU Group data. + return false; + } + + if (gpsGroup) + { + if (*reinterpret_cast(curField) != gpsGroup) + // Not the expected collection of field data types. + return false; + + curField += 2; + } + else if (groups & 0x08) + { + // There is unexpected GPS Group data. + return false; + } + + if (attitudeGroup) + { + if (*reinterpret_cast(curField) != attitudeGroup) + // Not the expected collection of field data types. + return false; + + curField += 2; + } + else if (groups & 0x10) + { + // There is unexpected Attitude Group data. + return false; + } + + if (insGroup) + { + if (*reinterpret_cast(curField) != insGroup) + // Not the expected collection of field data types. + return false; + + curField += 2; + } + else if (groups & 0x20) + { + // There is unexpected INS Group data. + return false; + } + + // Everything checks out. + return true; +} + +char* startAsciiPacketParse(char* packetStart, size_t& index) +{ + index = 7; + + return vnstrtok(packetStart, index); +} + +char* startAsciiResponsePacketParse(char* packetStart, size_t& index) +{ + startAsciiPacketParse(packetStart, index); + + return vnstrtok(packetStart, index); +} + +char* getNextData(char* str, size_t& startIndex) +{ + return vnstrtok(str, startIndex); +} + +char* vnstrtok(char* str, size_t& startIndex) +{ + size_t origIndex = startIndex; + + while (str[startIndex] != ',' && str[startIndex] != '*') + startIndex++; + + str[startIndex++] = '\0'; + + return str + origIndex; +} + +void Packet::ensureCanExtract(size_t numOfBytes) +{ + if (_curExtractLoc == 0) + // Determine the location to start extracting. + _curExtractLoc = countSetBits(_data[1]) * 2 + 2; + + if (_curExtractLoc + numOfBytes > _length - 2) + // About to overrun data. + throw invalid_operation(); +} + +uint8_t Packet::extractUint8() +{ + ensureCanExtract(sizeof(uint8_t)); + + uint8_t d = *reinterpret_cast(_data + _curExtractLoc); + + _curExtractLoc += sizeof(uint8_t); + + return d; +} + +int8_t Packet::extractInt8() +{ + ensureCanExtract(sizeof(int8_t)); + + int8_t d = *reinterpret_cast(_data + _curExtractLoc); + + _curExtractLoc += sizeof(int8_t); + + return d; +} + +uint16_t Packet::extractUint16() +{ + ensureCanExtract(sizeof(uint16_t)); + + uint16_t d; + + memcpy(&d, _data + _curExtractLoc, sizeof(uint16_t)); + + _curExtractLoc += sizeof(uint16_t); + + return stoh(d); +} + +uint32_t Packet::extractUint32() +{ + ensureCanExtract(sizeof(uint32_t)); + + uint32_t d; + + memcpy(&d, _data + _curExtractLoc, sizeof(uint32_t)); + + _curExtractLoc += sizeof(uint32_t); + + return stoh(d); +} + +uint64_t Packet::extractUint64() +{ + ensureCanExtract(sizeof(uint64_t)); + + uint64_t d; + + memcpy(&d, _data + _curExtractLoc, sizeof(uint64_t)); + + _curExtractLoc += sizeof(uint64_t); + + return stoh(d); +} + +float Packet::extractFloat() +{ + ensureCanExtract(sizeof(float)); + + float f; + + memcpy(&f, _data + _curExtractLoc, sizeof(float)); + + _curExtractLoc += sizeof(float); + + return f; +} + +vec3f Packet::extractVec3f() +{ + ensureCanExtract(3 * sizeof(float)); + + vec3f d; + + memcpy(&d.x, _data + _curExtractLoc, sizeof(float)); + memcpy(&d.y, _data + _curExtractLoc + sizeof(float), sizeof(float)); + memcpy(&d.z, _data + _curExtractLoc + 2 * sizeof(float), sizeof(float)); + + _curExtractLoc += 3 * sizeof(float); + + return d; +} + +vec3d Packet::extractVec3d() +{ + ensureCanExtract(3 * sizeof(double)); + + vec3d d; + + memcpy(&d.x, _data + _curExtractLoc, sizeof(double)); + memcpy(&d.y, _data + _curExtractLoc + sizeof(double), sizeof(double)); + memcpy(&d.z, _data + _curExtractLoc + 2 * sizeof(double), sizeof(double)); + + _curExtractLoc += 3 * sizeof(double); + + return d; +} + +vec4f Packet::extractVec4f() +{ + ensureCanExtract(4 * sizeof(float)); + + vec4f d; + + memcpy(&d.x, _data + _curExtractLoc, sizeof(float)); + memcpy(&d.y, _data + _curExtractLoc + sizeof(float), sizeof(float)); + memcpy(&d.z, _data + _curExtractLoc + 2 * sizeof(float), sizeof(float)); + memcpy(&d.w, _data + _curExtractLoc + 3 * sizeof(float), sizeof(float)); + + _curExtractLoc += 4 * sizeof(float); + + return d; +} + +mat3f Packet::extractMat3f() +{ + ensureCanExtract(9 * sizeof(float)); + + mat3f m; + + memcpy(&m.e00, _data + _curExtractLoc, sizeof(float)); + memcpy(&m.e10, _data + _curExtractLoc + sizeof(float), sizeof(float)); + memcpy(&m.e20, _data + _curExtractLoc + 2 * sizeof(float), sizeof(float)); + memcpy(&m.e01, _data + _curExtractLoc + 3 * sizeof(float), sizeof(float)); + memcpy(&m.e11, _data + _curExtractLoc + 4 * sizeof(float), sizeof(float)); + memcpy(&m.e21, _data + _curExtractLoc + 5 * sizeof(float), sizeof(float)); + memcpy(&m.e02, _data + _curExtractLoc + 6 * sizeof(float), sizeof(float)); + memcpy(&m.e12, _data + _curExtractLoc + 7 * sizeof(float), sizeof(float)); + memcpy(&m.e22, _data + _curExtractLoc + 8 * sizeof(float), sizeof(float)); + + _curExtractLoc += 9 * sizeof(float); + + return m; +} + +size_t Packet::finalizeCommand(ErrorDetectionMode errorDetectionMode, char *packet, size_t length) +{ + #if defined(_MSC_VER) + // Unable to use save version 'sprintf_s' since the length of 'packet' is unknown here. + #pragma warning(push) + #pragma warning(disable:4996) + #endif + + if (errorDetectionMode == ERRORDETECTIONMODE_CHECKSUM) + { + length += sprintf(packet + length, "*%02X\r\n", Checksum8::compute(packet + 1, length - 1)); + } + else if (errorDetectionMode == ERRORDETECTIONMODE_CRC) + { + length += sprintf(packet + length, "*%04X\r\n", Crc16::compute(packet + 1, length - 1)); + } + else + { + length += sprintf(packet + length, "*XX\r\n"); + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif + + return length; +} + +size_t Packet::genReadBinaryOutput1(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,75"); + #else + size_t length = sprintf(buffer, "$VNRRG,75"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadBinaryOutput2(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,76"); + #else + size_t length = sprintf(buffer, "$VNRRG,76"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadBinaryOutput3(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,77"); + #else + size_t length = sprintf(buffer, "$VNRRG,77"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + + +size_t writeBinaryOutput(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint8_t binaryOutputNumber, uint16_t asyncMode, uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, uint16_t imuField, uint16_t gpsField, uint16_t attitudeField, uint16_t insField) +{ + // First determine which groups are present. + uint16_t groups = 0; + if (commonField) + groups |= 0x0001; + if (timeField) + groups |= 0x0002; + if (imuField) + groups |= 0x0004; + if (gpsField) + groups |= 0x0008; + if (attitudeField) + groups |= 0x0010; + if (insField) + groups |= 0x0020; + + #if VN_HAVE_SECURE_CRT + int length = sprintf_s(buffer, size, "$VNWRG,%u,%u,%u,%X", 74 + binaryOutputNumber, asyncMode, rateDivisor, groups); + #else + int length = sprintf(buffer, "$VNWRG,%u,%u,%u,%X", 74 + binaryOutputNumber, asyncMode, rateDivisor, groups); + #endif + + if (commonField) + #if VN_HAVE_SECURE_CRT + length += sprintf_s(buffer + length, size - length, ",%X", commonField); + #else + length += sprintf(buffer + length, ",%X", commonField); + #endif + if (timeField) + #if VN_HAVE_SECURE_CRT + length += sprintf_s(buffer + length, size - length, ",%X", timeField); + #else + length += sprintf(buffer + length, ",%X", timeField); + #endif + if (imuField) + #if VN_HAVE_SECURE_CRT + length += sprintf_s(buffer + length, size - length, ",%X", imuField); + #else + length += sprintf(buffer + length, ",%X", imuField); + #endif + if (gpsField) + #if VN_HAVE_SECURE_CRT + length += sprintf_s(buffer + length, size - length, ",%X", gpsField); + #else + length += sprintf(buffer + length, ",%X", gpsField); + #endif + if (attitudeField) + #if VN_HAVE_SECURE_CRT + length += sprintf_s(buffer + length, size - length, ",%X", attitudeField); + #else + length += sprintf(buffer + length, ",%X", attitudeField); + #endif + if (insField) + #if VN_HAVE_SECURE_CRT + length += sprintf_s(buffer + length, size - length, ",%X", insField); + #else + length += sprintf(buffer + length, ",%X", insField); + #endif + + return Packet::finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteBinaryOutput1(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint16_t asyncMode, uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, uint16_t imuField, uint16_t gpsField, uint16_t attitudeField, uint16_t insField) +{ + return writeBinaryOutput(errorDetectionMode, buffer, size, 1, asyncMode, rateDivisor, commonField, timeField, imuField, gpsField, attitudeField, insField); +} + +size_t Packet::genWriteBinaryOutput2(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint16_t asyncMode, uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, uint16_t imuField, uint16_t gpsField, uint16_t attitudeField, uint16_t insField) +{ + return writeBinaryOutput(errorDetectionMode, buffer, size, 2, asyncMode, rateDivisor, commonField, timeField, imuField, gpsField, attitudeField, insField); +} + +size_t Packet::genWriteBinaryOutput3(ErrorDetectionMode errorDetectionMode, char* buffer, size_t size, uint16_t asyncMode, uint16_t rateDivisor, uint16_t commonField, uint16_t timeField, uint16_t imuField, uint16_t gpsField, uint16_t attitudeField, uint16_t insField) +{ + return writeBinaryOutput(errorDetectionMode, buffer, size, 3, asyncMode, rateDivisor, commonField, timeField, imuField, gpsField, attitudeField, insField); +} + + +size_t Packet::genWriteSettings(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWNV"); + #else + size_t length = sprintf(buffer, "$VNWNV"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genTare(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNTAR"); + #else + size_t length = sprintf(buffer, "$VNTAR"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genKnownMagneticDisturbance(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, bool isMagneticDisturbancePresent) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNKMD,%d", isMagneticDisturbancePresent ? 1 : 0); + #else + size_t length = sprintf(buffer, "$VNKMD,%d", isMagneticDisturbancePresent ? 1 : 0); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genKnownAccelerationDisturbance(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, bool isAccelerationDisturbancePresent) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNKAD,%d", isAccelerationDisturbancePresent ? 1 : 0); + #else + size_t length = sprintf(buffer, "$VNKAD,%d", isAccelerationDisturbancePresent ? 1 : 0); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genSetGyroBias(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNSGB"); + #else + size_t length = sprintf(buffer, "$VNSGB"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genRestoreFactorySettings(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRFS"); + #else + size_t length = sprintf(buffer, "$VNRFS"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReset(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRST"); + #else + size_t length = sprintf(buffer, "$VNRST"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadSerialBaudRate(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t port) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,05,%u", port); + #else + size_t length = sprintf(buffer, "$VNRRG,05,%u", port); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteSerialBaudRate(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint32_t baudrate, uint8_t port) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,05,%u,%u", baudrate, port); + #else + size_t length = sprintf(buffer, "$VNWRG,05,%u,%u", baudrate, port); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadAsyncDataOutputType(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t port) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,06,%u", port); + #else + size_t length = sprintf(buffer, "$VNRRG,06,%u", port); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteAsyncDataOutputType(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint32_t ador, uint8_t port) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,06,%u,%u", ador, port); + #else + size_t length = sprintf(buffer, "$VNWRG,06,%u,%u", ador, port); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadAsyncDataOutputFrequency(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t port) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,07,%u", port); + #else + size_t length = sprintf(buffer, "$VNRRG,07,%u", port); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteAsyncDataOutputFrequency(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint32_t adof, uint8_t port) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,07,%u,%u", adof, port); + #else + size_t length = sprintf(buffer, "$VNWRG,07,%u,%u", adof, port); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteFilterMeasurementsVarianceParameters(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, float angularWalkVariance, vec3f angularRateVariance, vec3f magneticVariance, vec3f accelerationVariance) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,22,%E,%E,%E,%E,%f,%f,%f,%f,%f,%f", + #else + size_t length = sprintf(buffer, "$VNWRG,22,%E,%E,%E,%E,%f,%f,%f,%f,%f,%f", + #endif + angularWalkVariance, + angularRateVariance.x, + angularRateVariance.y, + angularRateVariance.z, + magneticVariance.x, + magneticVariance.y, + magneticVariance.z, + accelerationVariance.x, + accelerationVariance.y, + accelerationVariance.z); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadUserTag(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,00"); + #else + size_t length = sprintf(buffer, "$VNRRG,00"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteUserTag(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, string tag) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,00,%s" + #else + size_t length = sprintf(buffer, "$VNWRG,00,%s" + #endif +, + tag.c_str()); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadModelNumber(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,01"); + #else + size_t length = sprintf(buffer, "$VNRRG,01"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadHardwareRevision(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,02"); + #else + size_t length = sprintf(buffer, "$VNRRG,02"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadSerialNumber(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,03"); + #else + size_t length = sprintf(buffer, "$VNRRG,03"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadFirmwareVersion(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,04"); + #else + size_t length = sprintf(buffer, "$VNRRG,04"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadSerialBaudRate(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,05"); + #else + size_t length = sprintf(buffer, "$VNRRG,05"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteSerialBaudRate(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint32_t baudrate) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,05,%u" + #else + size_t length = sprintf(buffer, "$VNWRG,05,%u" + #endif +, + baudrate); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadAsyncDataOutputType(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,06"); + #else + size_t length = sprintf(buffer, "$VNRRG,06"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteAsyncDataOutputType(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint32_t ador) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,06,%u" + #else + size_t length = sprintf(buffer, "$VNWRG,06,%u" + #endif +, + ador); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadAsyncDataOutputFrequency(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,07"); + #else + size_t length = sprintf(buffer, "$VNRRG,07"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteAsyncDataOutputFrequency(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint32_t adof) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,07,%u" + #else + size_t length = sprintf(buffer, "$VNWRG,07,%u" + #endif +, + adof); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadYawPitchRoll(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,08"); + #else + size_t length = sprintf(buffer, "$VNRRG,08"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadAttitudeQuaternion(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,09"); + #else + size_t length = sprintf(buffer, "$VNRRG,09"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadQuaternionMagneticAccelerationAndAngularRates(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,15"); + #else + size_t length = sprintf(buffer, "$VNRRG,15"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadMagneticMeasurements(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,17"); + #else + size_t length = sprintf(buffer, "$VNRRG,17"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadAccelerationMeasurements(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,18"); + #else + size_t length = sprintf(buffer, "$VNRRG,18"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadAngularRateMeasurements(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,19"); + #else + size_t length = sprintf(buffer, "$VNRRG,19"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadMagneticAccelerationAndAngularRates(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,20"); + #else + size_t length = sprintf(buffer, "$VNRRG,20"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadMagneticAndGravityReferenceVectors(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,21"); + #else + size_t length = sprintf(buffer, "$VNRRG,21"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteMagneticAndGravityReferenceVectors(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f magRef, vec3f accRef) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,21,%f,%f,%f,%f,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,21,%f,%f,%f,%f,%f,%f" + #endif +, + magRef.x, + magRef.y, + magRef.z, + accRef.x, + accRef.y, + accRef.z); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadFilterMeasurementsVarianceParameters(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,22"); + #else + size_t length = sprintf(buffer, "$VNRRG,22"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadMagnetometerCompensation(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,23"); + #else + size_t length = sprintf(buffer, "$VNRRG,23"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteMagnetometerCompensation(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, mat3f c, vec3f b) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,23,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,23,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f" + #endif +, + c.e00, + c.e01, + c.e02, + c.e10, + c.e11, + c.e12, + c.e20, + c.e21, + c.e22, + b.x, + b.y, + b.z); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadFilterActiveTuningParameters(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,24"); + #else + size_t length = sprintf(buffer, "$VNRRG,24"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteFilterActiveTuningParameters(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, float magneticDisturbanceGain, float accelerationDisturbanceGain, float magneticDisturbanceMemory, float accelerationDisturbanceMemory) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,24,%f,%f,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,24,%f,%f,%f,%f" + #endif +, + magneticDisturbanceGain, + accelerationDisturbanceGain, + magneticDisturbanceMemory, + accelerationDisturbanceMemory); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadAccelerationCompensation(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,25"); + #else + size_t length = sprintf(buffer, "$VNRRG,25"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteAccelerationCompensation(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, mat3f c, vec3f b) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,25,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,25,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f" + #endif +, + c.e00, + c.e01, + c.e02, + c.e10, + c.e11, + c.e12, + c.e20, + c.e21, + c.e22, + b.x, + b.y, + b.z); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadReferenceFrameRotation(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,26"); + #else + size_t length = sprintf(buffer, "$VNRRG,26"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteReferenceFrameRotation(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, mat3f c) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,26,%f,%f,%f,%f,%f,%f,%f,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,26,%f,%f,%f,%f,%f,%f,%f,%f,%f" + #endif +, + c.e00, + c.e01, + c.e02, + c.e10, + c.e11, + c.e12, + c.e20, + c.e21, + c.e22); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadYawPitchRollMagneticAccelerationAndAngularRates(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,27"); + #else + size_t length = sprintf(buffer, "$VNRRG,27"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadCommunicationProtocolControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,30"); + #else + size_t length = sprintf(buffer, "$VNRRG,30"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteCommunicationProtocolControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t serialCount, uint8_t serialStatus, uint8_t spiCount, uint8_t spiStatus, uint8_t serialChecksum, uint8_t spiChecksum, uint8_t errorMode) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,30,%u,%u,%u,%u,%u,%u,%u" + #else + size_t length = sprintf(buffer, "$VNWRG,30,%u,%u,%u,%u,%u,%u,%u" + #endif +, + serialCount, + serialStatus, + spiCount, + spiStatus, + serialChecksum, + spiChecksum, + errorMode); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadSynchronizationControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,32"); + #else + size_t length = sprintf(buffer, "$VNRRG,32"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteSynchronizationControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t syncInMode, uint8_t syncInEdge, uint16_t syncInSkipFactor, uint8_t syncOutMode, uint8_t syncOutPolarity, uint16_t syncOutSkipFactor, uint32_t syncOutPulseWidth) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,32,%u,%u,%u,0,%u,%u,%u,%u,0" + #else + size_t length = sprintf(buffer, "$VNWRG,32,%u,%u,%u,0,%u,%u,%u,%u,0" + #endif +, + syncInMode, + syncInEdge, + syncInSkipFactor, + syncOutMode, + syncOutPolarity, + syncOutSkipFactor, + syncOutPulseWidth); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadSynchronizationStatus(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,33"); + #else + size_t length = sprintf(buffer, "$VNRRG,33"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteSynchronizationStatus(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint32_t syncInCount, uint32_t syncInTime, uint32_t syncOutCount) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,33,%u,%u,%u" + #else + size_t length = sprintf(buffer, "$VNWRG,33,%u,%u,%u" + #endif +, + syncInCount, + syncInTime, + syncOutCount); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadFilterBasicControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,34"); + #else + size_t length = sprintf(buffer, "$VNRRG,34"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteFilterBasicControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t magMode, uint8_t extMagMode, uint8_t extAccMode, uint8_t extGyroMode, vec3f gyroLimit) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,34,%u,%u,%u,%u,%f,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,34,%u,%u,%u,%u,%f,%f,%f" + #endif +, + magMode, + extMagMode, + extAccMode, + extGyroMode, + gyroLimit.x, + gyroLimit.y, + gyroLimit.z); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadVpeBasicControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,35"); + #else + size_t length = sprintf(buffer, "$VNRRG,35"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteVpeBasicControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t enable, uint8_t headingMode, uint8_t filteringMode, uint8_t tuningMode) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,35,%u,%u,%u,%u" + #else + size_t length = sprintf(buffer, "$VNWRG,35,%u,%u,%u,%u" + #endif +, + enable, + headingMode, + filteringMode, + tuningMode); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadVpeMagnetometerBasicTuning(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,36"); + #else + size_t length = sprintf(buffer, "$VNRRG,36"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteVpeMagnetometerBasicTuning(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f baseTuning, vec3f adaptiveTuning, vec3f adaptiveFiltering) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,36,%f,%f,%f,%f,%f,%f,%f,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,36,%f,%f,%f,%f,%f,%f,%f,%f,%f" + #endif +, + baseTuning.x, + baseTuning.y, + baseTuning.z, + adaptiveTuning.x, + adaptiveTuning.y, + adaptiveTuning.z, + adaptiveFiltering.x, + adaptiveFiltering.y, + adaptiveFiltering.z); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadVpeMagnetometerAdvancedTuning(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,37"); + #else + size_t length = sprintf(buffer, "$VNRRG,37"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteVpeMagnetometerAdvancedTuning(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f minFiltering, vec3f maxFiltering, float maxAdaptRate, float disturbanceWindow, float maxTuning) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,37,%f,%f,%f,%f,%f,%f,%f,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,37,%f,%f,%f,%f,%f,%f,%f,%f,%f" + #endif +, + minFiltering.x, + minFiltering.y, + minFiltering.z, + maxFiltering.x, + maxFiltering.y, + maxFiltering.z, + maxAdaptRate, + disturbanceWindow, + maxTuning); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadVpeAccelerometerBasicTuning(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,38"); + #else + size_t length = sprintf(buffer, "$VNRRG,38"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteVpeAccelerometerBasicTuning(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f baseTuning, vec3f adaptiveTuning, vec3f adaptiveFiltering) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,38,%f,%f,%f,%f,%f,%f,%f,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,38,%f,%f,%f,%f,%f,%f,%f,%f,%f" + #endif +, + baseTuning.x, + baseTuning.y, + baseTuning.z, + adaptiveTuning.x, + adaptiveTuning.y, + adaptiveTuning.z, + adaptiveFiltering.x, + adaptiveFiltering.y, + adaptiveFiltering.z); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadVpeAccelerometerAdvancedTuning(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,39"); + #else + size_t length = sprintf(buffer, "$VNRRG,39"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteVpeAccelerometerAdvancedTuning(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f minFiltering, vec3f maxFiltering, float maxAdaptRate, float disturbanceWindow, float maxTuning) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,39,%f,%f,%f,%f,%f,%f,%f,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,39,%f,%f,%f,%f,%f,%f,%f,%f,%f" + #endif +, + minFiltering.x, + minFiltering.y, + minFiltering.z, + maxFiltering.x, + maxFiltering.y, + maxFiltering.z, + maxAdaptRate, + disturbanceWindow, + maxTuning); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadVpeGryoBasicTuning(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,40"); + #else + size_t length = sprintf(buffer, "$VNRRG,40"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteVpeGryoBasicTuning(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f angularWalkVariance, vec3f baseTuning, vec3f adaptiveTuning) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,40,%f,%f,%f,%f,%f,%f,%f,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,40,%f,%f,%f,%f,%f,%f,%f,%f,%f" + #endif +, + angularWalkVariance.x, + angularWalkVariance.y, + angularWalkVariance.z, + baseTuning.x, + baseTuning.y, + baseTuning.z, + adaptiveTuning.x, + adaptiveTuning.y, + adaptiveTuning.z); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadFilterStartupGyroBias(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,43"); + #else + size_t length = sprintf(buffer, "$VNRRG,43"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteFilterStartupGyroBias(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f bias) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,43,%f,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,43,%f,%f,%f" + #endif +, + bias.x, + bias.y, + bias.z); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadMagnetometerCalibrationControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,44"); + #else + size_t length = sprintf(buffer, "$VNRRG,44"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteMagnetometerCalibrationControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t hsiMode, uint8_t hsiOutput, uint8_t convergeRate) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,44,%u,%u,%u" + #else + size_t length = sprintf(buffer, "$VNWRG,44,%u,%u,%u" + #endif +, + hsiMode, + hsiOutput, + convergeRate); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadCalculatedMagnetometerCalibration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,47"); + #else + size_t length = sprintf(buffer, "$VNRRG,47"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadIndoorHeadingModeControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,48"); + #else + size_t length = sprintf(buffer, "$VNRRG,48"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteIndoorHeadingModeControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, float maxRateError) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,48,%f,0" + #else + size_t length = sprintf(buffer, "$VNWRG,48,%f,0" + #endif +, + maxRateError); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadVelocityCompensationMeasurement(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,50"); + #else + size_t length = sprintf(buffer, "$VNRRG,50"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteVelocityCompensationMeasurement(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f velocity) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,50,%f,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,50,%f,%f,%f" + #endif +, + velocity.x, + velocity.y, + velocity.z); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadVelocityCompensationControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,51"); + #else + size_t length = sprintf(buffer, "$VNRRG,51"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteVelocityCompensationControl(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t mode, float velocityTuning, float rateTuning) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,51,%u,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,51,%u,%f,%f" + #endif +, + mode, + velocityTuning, + rateTuning); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadVelocityCompensationStatus(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,52"); + #else + size_t length = sprintf(buffer, "$VNRRG,52"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadImuMeasurements(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,54"); + #else + size_t length = sprintf(buffer, "$VNRRG,54"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadGpsConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,55"); + #else + size_t length = sprintf(buffer, "$VNRRG,55"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteGpsConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t mode, uint8_t ppsSource) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,55,%u,%u,5,0,0" + #else + size_t length = sprintf(buffer, "$VNWRG,55,%u,%u,5,0,0" + #endif +, + mode, + ppsSource); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadGpsAntennaOffset(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,57"); + #else + size_t length = sprintf(buffer, "$VNRRG,57"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteGpsAntennaOffset(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f position) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,57,%f,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,57,%f,%f,%f" + #endif +, + position.x, + position.y, + position.z); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadGpsSolutionLla(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,58"); + #else + size_t length = sprintf(buffer, "$VNRRG,58"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadGpsSolutionEcef(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,59"); + #else + size_t length = sprintf(buffer, "$VNRRG,59"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadInsSolutionLla(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,63"); + #else + size_t length = sprintf(buffer, "$VNRRG,63"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadInsSolutionEcef(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,64"); + #else + size_t length = sprintf(buffer, "$VNRRG,64"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadInsBasicConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,67"); + #else + size_t length = sprintf(buffer, "$VNRRG,67"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteInsBasicConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t scenario, uint8_t ahrsAiding, uint8_t estBaseline) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,67,%u,%u,%u,0" + #else + size_t length = sprintf(buffer, "$VNWRG,67,%u,%u,%u,0" + #endif +, + scenario, + ahrsAiding, + estBaseline); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadInsAdvancedConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,68"); + #else + size_t length = sprintf(buffer, "$VNRRG,68"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteInsAdvancedConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t useMag, uint8_t usePres, uint8_t posAtt, uint8_t velAtt, uint8_t velBias, uint8_t useFoam, uint8_t gpsCovType, uint8_t velCount, float velInit, float moveOrigin, float gpsTimeout, float deltaLimitPos, float deltaLimitVel, float minPosUncertainty, float minVelUncertainty) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,68,%u,%u,%u,%u,%u,%u,%u,%u,%f,%f,%f,%f,%f,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,68,%u,%u,%u,%u,%u,%u,%u,%u,%f,%f,%f,%f,%f,%f,%f" + #endif +, + useMag, + usePres, + posAtt, + velAtt, + velBias, + useFoam, + gpsCovType, + velCount, + velInit, + moveOrigin, + gpsTimeout, + deltaLimitPos, + deltaLimitVel, + minPosUncertainty, + minVelUncertainty); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadInsStateLla(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,72"); + #else + size_t length = sprintf(buffer, "$VNRRG,72"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadInsStateEcef(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,73"); + #else + size_t length = sprintf(buffer, "$VNRRG,73"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadStartupFilterBiasEstimate(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,74"); + #else + size_t length = sprintf(buffer, "$VNRRG,74"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteStartupFilterBiasEstimate(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f gyroBias, vec3f accelBias, float pressureBias) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,74,%f,%f,%f,%f,%f,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,74,%f,%f,%f,%f,%f,%f,%f" + #endif +, + gyroBias.x, + gyroBias.y, + gyroBias.z, + accelBias.x, + accelBias.y, + accelBias.z, + pressureBias); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadDeltaThetaAndDeltaVelocity(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,80"); + #else + size_t length = sprintf(buffer, "$VNRRG,80"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadDeltaThetaAndDeltaVelocityConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,82"); + #else + size_t length = sprintf(buffer, "$VNRRG,82"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteDeltaThetaAndDeltaVelocityConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t integrationFrame, uint8_t gyroCompensation, uint8_t accelCompensation) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,82,%u,%u,%u,0,0" + #else + size_t length = sprintf(buffer, "$VNWRG,82,%u,%u,%u,0,0" + #endif +, + integrationFrame, + gyroCompensation, + accelCompensation); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadReferenceVectorConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,83"); + #else + size_t length = sprintf(buffer, "$VNRRG,83"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteReferenceVectorConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint8_t useMagModel, uint8_t useGravityModel, uint32_t recalcThreshold, float year, vec3d position) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,83,%u,%u,0,0,%u,%f,%f,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,83,%u,%u,0,0,%u,%f,%f,%f,%f" + #endif +, + useMagModel, + useGravityModel, + recalcThreshold, + year, + position.x, + position.y, + position.z); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadGyroCompensation(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,84"); + #else + size_t length = sprintf(buffer, "$VNRRG,84"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteGyroCompensation(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, mat3f c, vec3f b) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,84,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,84,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f" + #endif +, + c.e00, + c.e01, + c.e02, + c.e10, + c.e11, + c.e12, + c.e20, + c.e21, + c.e22, + b.x, + b.y, + b.z); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadImuFilteringConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,85"); + #else + size_t length = sprintf(buffer, "$VNRRG,85"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteImuFilteringConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint16_t magWindowSize, uint16_t accelWindowSize, uint16_t gyroWindowSize, uint16_t tempWindowSize, uint16_t presWindowSize, uint8_t magFilterMode, uint8_t accelFilterMode, uint8_t gyroFilterMode, uint8_t tempFilterMode, uint8_t presFilterMode) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,85,%u,%u,%u,%u,%u,%u,%u,%u,%u,%u" + #else + size_t length = sprintf(buffer, "$VNWRG,85,%u,%u,%u,%u,%u,%u,%u,%u,%u,%u" + #endif +, + magWindowSize, + accelWindowSize, + gyroWindowSize, + tempWindowSize, + presWindowSize, + magFilterMode, + accelFilterMode, + gyroFilterMode, + tempFilterMode, + presFilterMode); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadGpsCompassBaseline(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,93"); + #else + size_t length = sprintf(buffer, "$VNRRG,93"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteGpsCompassBaseline(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, vec3f position, vec3f uncertainty) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,93,%f,%f,%f,%f,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,93,%f,%f,%f,%f,%f,%f" + #endif +, + position.x, + position.y, + position.z, + uncertainty.x, + uncertainty.y, + uncertainty.z); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadGpsCompassEstimatedBaseline(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,97"); + #else + size_t length = sprintf(buffer, "$VNRRG,97"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadImuRateConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,227"); + #else + size_t length = sprintf(buffer, "$VNRRG,227"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genWriteImuRateConfiguration(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size, uint16_t imuRate, uint16_t navDivisor, float filterTargetRate, float filterMinRate) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNWRG,227,%u,%u,%f,%f" + #else + size_t length = sprintf(buffer, "$VNWRG,227,%u,%u,%f,%f" + #endif +, + imuRate, + navDivisor, + filterTargetRate, + filterMinRate); + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadYawPitchRollTrueBodyAccelerationAndAngularRates(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,239"); + #else + size_t length = sprintf(buffer, "$VNRRG,239"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +size_t Packet::genReadYawPitchRollTrueInertialAccelerationAndAngularRates(ErrorDetectionMode errorDetectionMode, char *buffer, size_t size) +{ + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s(buffer, size, "$VNRRG,240"); + #else + size_t length = sprintf(buffer, "$VNRRG,240"); + #endif + + return finalizeCommand(errorDetectionMode, buffer, length); +} + +void Packet::parseVNYPR(vec3f* yawPitchRoll) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + yawPitchRoll->x = ATOFF; NEXT + yawPitchRoll->y = ATOFF; NEXT + yawPitchRoll->z = ATOFF; +} + +void Packet::parseVNQTN(vec4f* quaternion) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + quaternion->x = ATOFF; NEXT + quaternion->y = ATOFF; NEXT + quaternion->z = ATOFF; NEXT + quaternion->w = ATOFF; +} + +#ifdef INTERNAL + +void Packet::parseVNQTM(vec4f *quaternion, vec3f *magnetic) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + quaternion->x = ATOFF; NEXT + quaternion->y = ATOFF; NEXT + quaternion->z = ATOFF; NEXT + quaternion->w = ATOFF; NEXT + magnetic->x = ATOFF; NEXT + magnetic->y = ATOFF; NEXT + magnetic->z = ATOFF; +} + +void Packet::parseVNQTA(vec4f* quaternion, vec3f* acceleration) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + quaternion->x = ATOFF; NEXT + quaternion->y = ATOFF; NEXT + quaternion->z = ATOFF; NEXT + quaternion->w = ATOFF; NEXT + acceleration->x = ATOFF; NEXT + acceleration->y = ATOFF; NEXT + acceleration->z = ATOFF; +} + +void Packet::parseVNQTR(vec4f* quaternion, vec3f* angularRate) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + quaternion->x = ATOFF; NEXT + quaternion->y = ATOFF; NEXT + quaternion->z = ATOFF; NEXT + quaternion->w = ATOFF; NEXT + angularRate->x = ATOFF; NEXT + angularRate->y = ATOFF; NEXT + angularRate->z = ATOFF; +} + +void Packet::parseVNQMA(vec4f* quaternion, vec3f* magnetic, vec3f* acceleration) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + quaternion->x = ATOFF; NEXT + quaternion->y = ATOFF; NEXT + quaternion->z = ATOFF; NEXT + quaternion->w = ATOFF; NEXT + magnetic->x = ATOFF; NEXT + magnetic->y = ATOFF; NEXT + magnetic->z = ATOFF; NEXT + acceleration->x = ATOFF; NEXT + acceleration->y = ATOFF; NEXT + acceleration->z = ATOFF; +} + +void Packet::parseVNQAR(vec4f* quaternion, vec3f* acceleration, vec3f* angularRate) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + quaternion->x = ATOFF; NEXT + quaternion->y = ATOFF; NEXT + quaternion->z = ATOFF; NEXT + quaternion->w = ATOFF; NEXT + acceleration->x = ATOFF; NEXT + acceleration->y = ATOFF; NEXT + acceleration->z = ATOFF; NEXT + angularRate->x = ATOFF; NEXT + angularRate->y = ATOFF; NEXT + angularRate->z = ATOFF; +} + +#endif + +void Packet::parseVNQMR(vec4f* quaternion, vec3f* magnetic, vec3f* acceleration, vec3f* angularRate) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + quaternion->x = ATOFF; NEXT + quaternion->y = ATOFF; NEXT + quaternion->z = ATOFF; NEXT + quaternion->w = ATOFF; NEXT + magnetic->x = ATOFF; NEXT + magnetic->y = ATOFF; NEXT + magnetic->z = ATOFF; NEXT + acceleration->x = ATOFF; NEXT + acceleration->y = ATOFF; NEXT + acceleration->z = ATOFF; NEXT + angularRate->x = ATOFF; NEXT + angularRate->y = ATOFF; NEXT + angularRate->z = ATOFF; +} + +#ifdef INTERNAL + +void Packet::parseVNDCM(mat3f* dcm) +{ + // TODO: Implement. + /* + result = strtok(buffer, delims); + result = strtok(0, delims); + if (result == NULL) + return; + data.dcm.c00 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.dcm.c01 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.dcm.c02 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.dcm.c10 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.dcm.c11 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.dcm.c12 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.dcm.c20 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.dcm.c21 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.dcm.c22 = atof(result); + */ +} + +#endif + +void Packet::parseVNMAG(vec3f* magnetic) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + magnetic->x = ATOFF; NEXT + magnetic->y = ATOFF; NEXT + magnetic->z = ATOFF; +} + +void Packet::parseVNACC(vec3f* acceleration) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + acceleration->x = ATOFF; NEXT + acceleration->y = ATOFF; NEXT + acceleration->z = ATOFF; +} + +void Packet::parseVNGYR(vec3f* angularRate) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + angularRate->x = ATOFF; NEXT + angularRate->y = ATOFF; NEXT + angularRate->z = ATOFF; +} + +void Packet::parseVNMAR(vec3f* magnetic, vec3f* acceleration, vec3f* angularRate) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + magnetic->x = ATOFF; NEXT + magnetic->y = ATOFF; NEXT + magnetic->z = ATOFF; NEXT + acceleration->x = ATOFF; NEXT + acceleration->y = ATOFF; NEXT + acceleration->z = ATOFF; NEXT + angularRate->x = ATOFF; NEXT + angularRate->y = ATOFF; NEXT + angularRate->z = ATOFF; +} + +void Packet::parseVNYMR(vec3f* yawPitchRoll, vec3f* magnetic, vec3f* acceleration, vec3f* angularRate) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + yawPitchRoll->x = ATOFF; NEXT + yawPitchRoll->y = ATOFF; NEXT + yawPitchRoll->z = ATOFF; NEXT + magnetic->x = ATOFF; NEXT + magnetic->y = ATOFF; NEXT + magnetic->z = ATOFF; NEXT + acceleration->x = ATOFF; NEXT + acceleration->y = ATOFF; NEXT + acceleration->z = ATOFF; NEXT + angularRate->x = ATOFF; NEXT + angularRate->y = ATOFF; NEXT + angularRate->z = ATOFF; +} + +#ifdef INTERNAL + +void Packet::parseVNYCM(vec3f* yawPitchRoll, vec3f* magnetic, vec3f* acceleration, vec3f* angularRate, float* temperature) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + yawPitchRoll->x = ATOFF; NEXT + yawPitchRoll->y = ATOFF; NEXT + yawPitchRoll->z = ATOFF; NEXT + magnetic->x = ATOFF; NEXT + magnetic->y = ATOFF; NEXT + magnetic->z = ATOFF; NEXT + acceleration->x = ATOFF; NEXT + acceleration->y = ATOFF; NEXT + acceleration->z = ATOFF; NEXT + angularRate->x = ATOFF; NEXT + angularRate->y = ATOFF; NEXT + angularRate->z = ATOFF; NEXT + *temperature = ATOFF; +} + +#endif + +void Packet::parseVNYBA(vec3f* yawPitchRoll, vec3f* accelerationBody, vec3f* angularRate) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + yawPitchRoll->x = ATOFF; NEXT + yawPitchRoll->y = ATOFF; NEXT + yawPitchRoll->z = ATOFF; NEXT + accelerationBody->x = ATOFF; NEXT + accelerationBody->y = ATOFF; NEXT + accelerationBody->z = ATOFF; NEXT + angularRate->x = ATOFF; NEXT + angularRate->y = ATOFF; NEXT + angularRate->z = ATOFF; +} + +void Packet::parseVNYIA(vec3f* yawPitchRoll, vec3f* accelerationInertial, vec3f* angularRate) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + yawPitchRoll->x = ATOFF; NEXT + yawPitchRoll->y = ATOFF; NEXT + yawPitchRoll->z = ATOFF; NEXT + accelerationInertial->x = ATOFF; NEXT + accelerationInertial->y = ATOFF; NEXT + accelerationInertial->z = ATOFF; NEXT + angularRate->x = ATOFF; NEXT + angularRate->y = ATOFF; NEXT + angularRate->z = ATOFF; +} + +#ifdef INTERNAL + +void Packet::parseVNICM(vec3f* yawPitchRoll, vec3f* magnetic, vec3f* accelerationInertial, vec3f* angularRate) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + yawPitchRoll->x = ATOFF; NEXT + yawPitchRoll->y = ATOFF; NEXT + yawPitchRoll->z = ATOFF; NEXT + magnetic->x = ATOFF; NEXT + magnetic->y = ATOFF; NEXT + magnetic->z = ATOFF; NEXT + accelerationInertial->x = ATOFF; NEXT + accelerationInertial->y = ATOFF; NEXT + accelerationInertial->z = ATOFF; NEXT + angularRate->x = ATOFF; NEXT + angularRate->y = ATOFF; NEXT + angularRate->z = ATOFF; +} + +#endif + +void Packet::parseVNIMU(vec3f* magneticUncompensated, vec3f* accelerationUncompensated, vec3f* angularRateUncompensated, float* temperature, float* pressure) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + magneticUncompensated->x = ATOFF; NEXT + magneticUncompensated->y = ATOFF; NEXT + magneticUncompensated->z = ATOFF; NEXT + accelerationUncompensated->x = ATOFF; NEXT + accelerationUncompensated->y = ATOFF; NEXT + accelerationUncompensated->z = ATOFF; NEXT + angularRateUncompensated->x = ATOFF; NEXT + angularRateUncompensated->y = ATOFF; NEXT + angularRateUncompensated->z = ATOFF; NEXT + *temperature = ATOFF; NEXT + *pressure = ATOFF; +} + +void Packet::parseVNGPS(double* time, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, vec3d* lla, vec3f* nedVel, vec3f* nedAcc, float* speedAcc, float* timeAcc) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + *time = ATOFD; NEXT + *week = static_cast(atoi(result)); NEXT + *gpsFix = static_cast(atoi(result)); NEXT + *numSats = static_cast(atoi(result)); NEXT + lla->x = ATOFD; NEXT + lla->y = ATOFD; NEXT + lla->z = ATOFD; NEXT + nedVel->x = ATOFF; NEXT + nedVel->y = ATOFF; NEXT + nedVel->z = ATOFF; NEXT + nedAcc->x = ATOFF; NEXT + nedAcc->y = ATOFF; NEXT + nedAcc->z = ATOFF; NEXT + *speedAcc = ATOFF; NEXT + *timeAcc = ATOFF; +} + +void Packet::parseVNINS(double* time, uint16_t* week, uint16_t* status, vec3f* yawPitchRoll, vec3d* lla, vec3f* nedVel, float* attUncertainty, float* posUncertainty, float* velUncertainty) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + *time = ATOFD; NEXT + *week = static_cast(atoi(result)); NEXT + *status = static_cast(atoi(result)); NEXT + yawPitchRoll->x = ATOFF; NEXT + yawPitchRoll->y = ATOFF; NEXT + yawPitchRoll->z = ATOFF; NEXT + lla->x = ATOFD; NEXT + lla->y = ATOFD; NEXT + lla->z = ATOFD; NEXT + nedVel->x = ATOFF; NEXT + nedVel->y = ATOFF; NEXT + nedVel->z = ATOFF; NEXT + *attUncertainty = ATOFF; NEXT + *posUncertainty = ATOFF; NEXT + *velUncertainty = ATOFF; +} + +void Packet::parseVNINE(double* time, uint16_t* week, uint16_t* status, vec3f* ypr, vec3d* position, vec3f* velocity, float* attUncertainty, float* posUncertainty, float* velUncertainty) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + *time = ATOFD; NEXT + *week = static_cast(atoi(result)); NEXT + *status = static_cast(atoi(result)); NEXT + ypr->x = ATOFF; NEXT + ypr->y = ATOFF; NEXT + ypr->z = ATOFF; NEXT + position->x = ATOFD; NEXT + position->y = ATOFD; NEXT + position->z = ATOFD; NEXT + velocity->x = ATOFF; NEXT + velocity->y = ATOFF; NEXT + velocity->z = ATOFF; NEXT + *attUncertainty = ATOFF; NEXT + *posUncertainty = ATOFF; NEXT + *velUncertainty = ATOFF; +} + +void Packet::parseVNISL(vec3f* ypr, vec3d* lla, vec3f* velocity, vec3f* acceleration, vec3f* angularRate) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + ypr->x = ATOFF; NEXT + ypr->y = ATOFF; NEXT + ypr->z = ATOFF; NEXT + lla->x = ATOFD; NEXT + lla->y = ATOFD; NEXT + lla->z = ATOFD; NEXT + velocity->x = ATOFF; NEXT + velocity->y = ATOFF; NEXT + velocity->z = ATOFF; NEXT + acceleration->x = ATOFF; NEXT + acceleration->y = ATOFF; NEXT + acceleration->z = ATOFF; NEXT + angularRate->x = ATOFF; NEXT + angularRate->y = ATOFF; NEXT + angularRate->z = ATOFF; +} + +void Packet::parseVNISE(vec3f* ypr, vec3d* position, vec3f* velocity, vec3f* acceleration, vec3f* angularRate) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + ypr->x = ATOFF; NEXT + ypr->y = ATOFF; NEXT + ypr->z = ATOFF; NEXT + position->x = ATOFD; NEXT + position->y = ATOFD; NEXT + position->z = ATOFD; NEXT + velocity->x = ATOFF; NEXT + velocity->y = ATOFF; NEXT + velocity->z = ATOFF; NEXT + acceleration->x = ATOFF; NEXT + acceleration->y = ATOFF; NEXT + acceleration->z = ATOFF; NEXT + angularRate->x = ATOFF; NEXT + angularRate->y = ATOFF; NEXT + angularRate->z = ATOFF; +} + +#ifdef INTERNAL + +void Packet::parseVNRAW(vec3f *magneticVoltage, vec3f *accelerationVoltage, vec3f *angularRateVoltage, float* temperatureVoltage) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + magneticVoltage->x = ATOFF; NEXT + magneticVoltage->y = ATOFF; NEXT + magneticVoltage->z = ATOFF; NEXT + accelerationVoltage->x = ATOFF; NEXT + accelerationVoltage->y = ATOFF; NEXT + accelerationVoltage->z = ATOFF; NEXT + angularRateVoltage->x = ATOFF; NEXT + angularRateVoltage->y = ATOFF; NEXT + angularRateVoltage->z = ATOFF; NEXT + *temperatureVoltage = ATOFF; +} + +void Packet::parseVNCMV(vec3f* magneticUncompensated, vec3f* accelerationUncompensated, vec3f* angularRateUncompensated, float* temperature) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + magneticUncompensated->x = ATOFF; NEXT + magneticUncompensated->y = ATOFF; NEXT + magneticUncompensated->z = ATOFF; NEXT + accelerationUncompensated->x = ATOFF; NEXT + accelerationUncompensated->y = ATOFF; NEXT + accelerationUncompensated->z = ATOFF; NEXT + angularRateUncompensated->x = ATOFF; NEXT + angularRateUncompensated->y = ATOFF; NEXT + angularRateUncompensated->z = ATOFF; NEXT + *temperature = ATOFF; +} + +void Packet::parseVNSTV(vec4f* quaternion, vec3f* angularRateBias) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + quaternion->x = ATOFF; NEXT + quaternion->y = ATOFF; NEXT + quaternion->z = ATOFF; NEXT + quaternion->w = ATOFF; NEXT + angularRateBias->x = ATOFF; NEXT + angularRateBias->y = ATOFF; NEXT + angularRateBias->z = ATOFF; +} + +void Packet::parseVNCOV(vec3f* attitudeVariance, vec3f* angularRateBiasVariance) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + attitudeVariance->x = ATOFF; NEXT + attitudeVariance->y = ATOFF; NEXT + attitudeVariance->z = ATOFF; NEXT + angularRateBiasVariance->x = ATOFF; NEXT + angularRateBiasVariance->y = ATOFF; NEXT + angularRateBiasVariance->z = ATOFF; +} + +#endif + +void Packet::parseVNGPE(double* tow, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, vec3d* position, vec3f* velocity, vec3f* posAcc, float* speedAcc, float* timeAcc) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + *tow = ATOFD; NEXT + *week = static_cast(atoi(result)); NEXT + *gpsFix = static_cast(atoi(result)); NEXT + *numSats = static_cast(atoi(result)); NEXT + position->x = ATOFD; NEXT + position->y = ATOFD; NEXT + position->z = ATOFD; NEXT + velocity->x = ATOFF; NEXT + velocity->y = ATOFF; NEXT + velocity->z = ATOFF; NEXT + posAcc->x = ATOFF; NEXT + posAcc->y = ATOFF; NEXT + posAcc->z = ATOFF; NEXT + *speedAcc = ATOFF; NEXT + *timeAcc = ATOFF; +} + +void Packet::parseVNDTV(float* deltaTime, vec3f* deltaTheta, vec3f* deltaVelocity) +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + *deltaTime = ATOFF; NEXT + deltaTheta->x = ATOFF; NEXT + deltaTheta->y = ATOFF; NEXT + deltaTheta->z = ATOFF; NEXT + deltaVelocity->x = ATOFF; NEXT + deltaVelocity->y = ATOFF; NEXT + deltaVelocity->z = ATOFF; +} + +size_t Packet::computeBinaryPacketLength(char const* startOfPossibleBinaryPacket) +{ + char groupsPresent = startOfPossibleBinaryPacket[1]; + size_t runningPayloadLength = 2; // Start of packet character plus groups present field. + const char* pCurrentGroupField = startOfPossibleBinaryPacket + 2; + + if (groupsPresent & 0x01) + { + runningPayloadLength += 2 + computeNumOfBytesForBinaryGroupPayload(BINARYGROUP_COMMON, stoh(*reinterpret_cast(pCurrentGroupField))); + pCurrentGroupField += 2; + } + + if (groupsPresent & 0x02) + { + runningPayloadLength += 2 + computeNumOfBytesForBinaryGroupPayload(BINARYGROUP_TIME, stoh(*reinterpret_cast(pCurrentGroupField))); + pCurrentGroupField += 2; + } + + if (groupsPresent & 0x04) + { + runningPayloadLength += 2 + computeNumOfBytesForBinaryGroupPayload(BINARYGROUP_IMU, stoh(*reinterpret_cast(pCurrentGroupField))); + pCurrentGroupField += 2; + } + + if (groupsPresent & 0x08) + { + runningPayloadLength += 2 + computeNumOfBytesForBinaryGroupPayload(BINARYGROUP_GPS, stoh(*reinterpret_cast(pCurrentGroupField))); + pCurrentGroupField += 2; + } + + if (groupsPresent & 0x10) + { + runningPayloadLength += 2 + computeNumOfBytesForBinaryGroupPayload(BINARYGROUP_ATTITUDE, stoh(*reinterpret_cast(pCurrentGroupField))); + pCurrentGroupField += 2; + } + + if (groupsPresent & 0x20) + { + runningPayloadLength += 2 + computeNumOfBytesForBinaryGroupPayload(BINARYGROUP_INS, stoh(*reinterpret_cast(pCurrentGroupField))); + pCurrentGroupField += 2; + } + + return runningPayloadLength + 2; // Add 2 bytes for CRC. +} + +size_t Packet::computeNumOfBytesForBinaryGroupPayload(BinaryGroup group, uint16_t groupField) +{ + size_t runningLength = 0; + + // Determine which group is present. + size_t groupIndex = 0; + for (size_t i = 0; i < 8; i++, groupIndex++) + { + if ((static_cast(group) >> i) & 0x01) + break; + } + + for (size_t i = 0; i < sizeof(uint16_t) * 8; i++) + { + if ((groupField >> i) & 1) + { + runningLength += BinaryGroupLengths[groupIndex][i]; + } + } + + return runningLength; +} + +SensorError Packet::parseError() +{ + size_t parseIndex; + + char* result = startAsciiPacketParse(_data, parseIndex); + + return static_cast(to_uint8_from_hexstr(result)); +} + +uint8_t Packet::groups() +{ + return _data[1]; +} + +uint16_t Packet::groupField(size_t index) +{ + uint16_t gf; + char* groupStart = _data + index * sizeof(uint16_t) + 2; + + std::memcpy(&gf, groupStart, sizeof(uint16_t)); + + return stoh(gf); +} + +void Packet::parseBinaryOutput( + uint16_t* asyncMode, + uint16_t* rateDivisor, + uint16_t* outputGroup, + uint16_t* commonField, + uint16_t* timeField, + uint16_t* imuField, + uint16_t* gpsField, + uint16_t* attitudeField, + uint16_t* insField) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *commonField = 0; + *timeField = 0; + *imuField = 0; + *gpsField = 0; + *attitudeField = 0; + *insField = 0; + + *asyncMode = ATOU16; NEXT + *rateDivisor = ATOU16; NEXT + *outputGroup = ATOU16; + if (*outputGroup & 0x0001) + { + NEXT + *commonField = ATOU16; + } + if (*outputGroup & 0x0002) + { + NEXT + *timeField = ATOU16; + } + if (*outputGroup & 0x0004) + { + NEXT + *imuField = ATOU16; + } + if (*outputGroup & 0x0008) + { + NEXT + *gpsField = ATOU16; + } + if (*outputGroup & 0x0010) + { + NEXT + *attitudeField = ATOU16; + } + if (*outputGroup & 0x0020) + { + NEXT + *insField = ATOU16; + } +} + +void Packet::parseUserTag(char* tag) +{ + size_t parseIndex; + + char* next = startAsciiPacketParse(_data, parseIndex); + + if (*(next + strlen(next) + 1) == '*') + { + tag[0] = '\0'; + return; + } + + next = getNextData(_data, parseIndex); + + #if defined(_MSC_VER) + //Unable to use strcpy_s since we do not have length of the output array. + #pragma warning(push) + #pragma warning(disable:4996) + #endif + + strcpy(tag, next); + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void Packet::parseModelNumber(char* productName) +{ + size_t parseIndex; + + char* next = startAsciiPacketParse(_data, parseIndex); + + if (*(next + strlen(next) + 1) == '*') + { + productName[0] = '\0'; + return; + } + + next = getNextData(_data, parseIndex); + + #if defined(_MSC_VER) + //Unable to use strcpy_s since we do not have length of the output array. + #pragma warning(push) + #pragma warning(disable:4996) + #endif + + strcpy(productName, next); + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void Packet::parseHardwareRevision(uint32_t* revision) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *revision = ATOU32; +} + +void Packet::parseSerialNumber(uint32_t* serialNum) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *serialNum = ATOU32; +} + +void Packet::parseFirmwareVersion(char* firmwareVersion) +{ + size_t parseIndex; + + char* next = startAsciiPacketParse(_data, parseIndex); + + if (*(next + strlen(next) + 1) == '*') + { + firmwareVersion[0] = '\0'; + return; + } + + next = getNextData(_data, parseIndex); + + #if defined(_MSC_VER) + //Unable to use strcpy_s since we do not have length of the output array. + #pragma warning(push) + #pragma warning(disable:4996) + #endif + + strcpy(firmwareVersion, next); + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void Packet::parseSerialBaudRate(uint32_t* baudrate) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *baudrate = ATOU32; +} + +void Packet::parseAsyncDataOutputType(uint32_t* ador) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *ador = ATOU32; +} + +void Packet::parseAsyncDataOutputFrequency(uint32_t* adof) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *adof = ATOU32; +} + +void Packet::parseYawPitchRoll(vec3f* yawPitchRoll) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + yawPitchRoll->x = ATOFF; NEXT + yawPitchRoll->y = ATOFF; NEXT + yawPitchRoll->z = ATOFF; +} + +void Packet::parseAttitudeQuaternion(vec4f* quat) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + quat->x = ATOFF; NEXT + quat->y = ATOFF; NEXT + quat->z = ATOFF; NEXT + quat->w = ATOFF; +} + +void Packet::parseQuaternionMagneticAccelerationAndAngularRates(vec4f* quat, vec3f* mag, vec3f* accel, vec3f* gyro) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + quat->x = ATOFF; NEXT + quat->y = ATOFF; NEXT + quat->z = ATOFF; NEXT + quat->w = ATOFF; NEXT + mag->x = ATOFF; NEXT + mag->y = ATOFF; NEXT + mag->z = ATOFF; NEXT + accel->x = ATOFF; NEXT + accel->y = ATOFF; NEXT + accel->z = ATOFF; NEXT + gyro->x = ATOFF; NEXT + gyro->y = ATOFF; NEXT + gyro->z = ATOFF; +} + +void Packet::parseMagneticMeasurements(vec3f* mag) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + mag->x = ATOFF; NEXT + mag->y = ATOFF; NEXT + mag->z = ATOFF; +} + +void Packet::parseAccelerationMeasurements(vec3f* accel) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + accel->x = ATOFF; NEXT + accel->y = ATOFF; NEXT + accel->z = ATOFF; +} + +void Packet::parseAngularRateMeasurements(vec3f* gyro) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + gyro->x = ATOFF; NEXT + gyro->y = ATOFF; NEXT + gyro->z = ATOFF; +} + +void Packet::parseMagneticAccelerationAndAngularRates(vec3f* mag, vec3f* accel, vec3f* gyro) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + mag->x = ATOFF; NEXT + mag->y = ATOFF; NEXT + mag->z = ATOFF; NEXT + accel->x = ATOFF; NEXT + accel->y = ATOFF; NEXT + accel->z = ATOFF; NEXT + gyro->x = ATOFF; NEXT + gyro->y = ATOFF; NEXT + gyro->z = ATOFF; +} + +void Packet::parseMagneticAndGravityReferenceVectors(vec3f* magRef, vec3f* accRef) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + magRef->x = ATOFF; NEXT + magRef->y = ATOFF; NEXT + magRef->z = ATOFF; NEXT + accRef->x = ATOFF; NEXT + accRef->y = ATOFF; NEXT + accRef->z = ATOFF; +} + +void Packet::parseFilterMeasurementsVarianceParameters(float* angularWalkVariance, vec3f* angularRateVariance, vec3f* magneticVariance, vec3f* accelerationVariance) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *angularWalkVariance = ATOFF; NEXT + angularRateVariance->x = ATOFF; NEXT + angularRateVariance->y = ATOFF; NEXT + angularRateVariance->z = ATOFF; NEXT + magneticVariance->x = ATOFF; NEXT + magneticVariance->y = ATOFF; NEXT + magneticVariance->z = ATOFF; NEXT + accelerationVariance->x = ATOFF; NEXT + accelerationVariance->y = ATOFF; NEXT + accelerationVariance->z = ATOFF; +} + +void Packet::parseMagnetometerCompensation(mat3f* c, vec3f* b) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + c->e00 = ATOFF; NEXT + c->e01 = ATOFF; NEXT + c->e02 = ATOFF; NEXT + c->e10 = ATOFF; NEXT + c->e11 = ATOFF; NEXT + c->e12 = ATOFF; NEXT + c->e20 = ATOFF; NEXT + c->e21 = ATOFF; NEXT + c->e22 = ATOFF; NEXT + b->x = ATOFF; NEXT + b->y = ATOFF; NEXT + b->z = ATOFF; +} + +void Packet::parseFilterActiveTuningParameters(float* magneticDisturbanceGain, float* accelerationDisturbanceGain, float* magneticDisturbanceMemory, float* accelerationDisturbanceMemory) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *magneticDisturbanceGain = ATOFF; NEXT + *accelerationDisturbanceGain = ATOFF; NEXT + *magneticDisturbanceMemory = ATOFF; NEXT + *accelerationDisturbanceMemory = ATOFF; +} + +void Packet::parseAccelerationCompensation(mat3f* c, vec3f* b) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + c->e00 = ATOFF; NEXT + c->e01 = ATOFF; NEXT + c->e02 = ATOFF; NEXT + c->e10 = ATOFF; NEXT + c->e11 = ATOFF; NEXT + c->e12 = ATOFF; NEXT + c->e20 = ATOFF; NEXT + c->e21 = ATOFF; NEXT + c->e22 = ATOFF; NEXT + b->x = ATOFF; NEXT + b->y = ATOFF; NEXT + b->z = ATOFF; +} + +void Packet::parseReferenceFrameRotation(mat3f* c) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + c->e00 = ATOFF; NEXT + c->e01 = ATOFF; NEXT + c->e02 = ATOFF; NEXT + c->e10 = ATOFF; NEXT + c->e11 = ATOFF; NEXT + c->e12 = ATOFF; NEXT + c->e20 = ATOFF; NEXT + c->e21 = ATOFF; NEXT + c->e22 = ATOFF; +} + +void Packet::parseYawPitchRollMagneticAccelerationAndAngularRates(vec3f* yawPitchRoll, vec3f* mag, vec3f* accel, vec3f* gyro) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + yawPitchRoll->x = ATOFF; NEXT + yawPitchRoll->y = ATOFF; NEXT + yawPitchRoll->z = ATOFF; NEXT + mag->x = ATOFF; NEXT + mag->y = ATOFF; NEXT + mag->z = ATOFF; NEXT + accel->x = ATOFF; NEXT + accel->y = ATOFF; NEXT + accel->z = ATOFF; NEXT + gyro->x = ATOFF; NEXT + gyro->y = ATOFF; NEXT + gyro->z = ATOFF; +} + +void Packet::parseCommunicationProtocolControl(uint8_t* serialCount, uint8_t* serialStatus, uint8_t* spiCount, uint8_t* spiStatus, uint8_t* serialChecksum, uint8_t* spiChecksum, uint8_t* errorMode) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *serialCount = ATOU8; NEXT + *serialStatus = ATOU8; NEXT + *spiCount = ATOU8; NEXT + *spiStatus = ATOU8; NEXT + *serialChecksum = ATOU8; NEXT + *spiChecksum = ATOU8; NEXT + *errorMode = ATOU8; +} + +void Packet::parseSynchronizationControl(uint8_t* syncInMode, uint8_t* syncInEdge, uint16_t* syncInSkipFactor, uint8_t* syncOutMode, uint8_t* syncOutPolarity, uint16_t* syncOutSkipFactor, uint32_t* syncOutPulseWidth) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *syncInMode = ATOU8; NEXT + *syncInEdge = ATOU8; NEXT + *syncInSkipFactor = ATOU16; NEXT + NEXT + *syncOutMode = ATOU8; NEXT + *syncOutPolarity = ATOU8; NEXT + *syncOutSkipFactor = ATOU16; NEXT + *syncOutPulseWidth = ATOU32; NEXT +} + +void Packet::parseSynchronizationStatus(uint32_t* syncInCount, uint32_t* syncInTime, uint32_t* syncOutCount) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *syncInCount = ATOU32; NEXT + *syncInTime = ATOU32; NEXT + *syncOutCount = ATOU32; +} + +void Packet::parseFilterBasicControl(uint8_t* magMode, uint8_t* extMagMode, uint8_t* extAccMode, uint8_t* extGyroMode, vec3f* gyroLimit) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *magMode = ATOU8; NEXT + *extMagMode = ATOU8; NEXT + *extAccMode = ATOU8; NEXT + *extGyroMode = ATOU8; NEXT + gyroLimit->x = ATOFF; NEXT + gyroLimit->y = ATOFF; NEXT + gyroLimit->z = ATOFF; +} + +void Packet::parseVpeBasicControl(uint8_t* enable, uint8_t* headingMode, uint8_t* filteringMode, uint8_t* tuningMode) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *enable = ATOU8; NEXT + *headingMode = ATOU8; NEXT + *filteringMode = ATOU8; NEXT + *tuningMode = ATOU8; +} + +void Packet::parseVpeMagnetometerBasicTuning(vec3f* baseTuning, vec3f* adaptiveTuning, vec3f* adaptiveFiltering) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + baseTuning->x = ATOFF; NEXT + baseTuning->y = ATOFF; NEXT + baseTuning->z = ATOFF; NEXT + adaptiveTuning->x = ATOFF; NEXT + adaptiveTuning->y = ATOFF; NEXT + adaptiveTuning->z = ATOFF; NEXT + adaptiveFiltering->x = ATOFF; NEXT + adaptiveFiltering->y = ATOFF; NEXT + adaptiveFiltering->z = ATOFF; +} + +void Packet::parseVpeMagnetometerAdvancedTuning(vec3f* minFiltering, vec3f* maxFiltering, float* maxAdaptRate, float* disturbanceWindow, float* maxTuning) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + minFiltering->x = ATOFF; NEXT + minFiltering->y = ATOFF; NEXT + minFiltering->z = ATOFF; NEXT + maxFiltering->x = ATOFF; NEXT + maxFiltering->y = ATOFF; NEXT + maxFiltering->z = ATOFF; NEXT + *maxAdaptRate = ATOFF; NEXT + *disturbanceWindow = ATOFF; NEXT + *maxTuning = ATOFF; +} + +void Packet::parseVpeAccelerometerBasicTuning(vec3f* baseTuning, vec3f* adaptiveTuning, vec3f* adaptiveFiltering) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + baseTuning->x = ATOFF; NEXT + baseTuning->y = ATOFF; NEXT + baseTuning->z = ATOFF; NEXT + adaptiveTuning->x = ATOFF; NEXT + adaptiveTuning->y = ATOFF; NEXT + adaptiveTuning->z = ATOFF; NEXT + adaptiveFiltering->x = ATOFF; NEXT + adaptiveFiltering->y = ATOFF; NEXT + adaptiveFiltering->z = ATOFF; +} + +void Packet::parseVpeAccelerometerAdvancedTuning(vec3f* minFiltering, vec3f* maxFiltering, float* maxAdaptRate, float* disturbanceWindow, float* maxTuning) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + minFiltering->x = ATOFF; NEXT + minFiltering->y = ATOFF; NEXT + minFiltering->z = ATOFF; NEXT + maxFiltering->x = ATOFF; NEXT + maxFiltering->y = ATOFF; NEXT + maxFiltering->z = ATOFF; NEXT + *maxAdaptRate = ATOFF; NEXT + *disturbanceWindow = ATOFF; NEXT + *maxTuning = ATOFF; +} + +void Packet::parseVpeGryoBasicTuning(vec3f* angularWalkVariance, vec3f* baseTuning, vec3f* adaptiveTuning) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + angularWalkVariance->x = ATOFF; NEXT + angularWalkVariance->y = ATOFF; NEXT + angularWalkVariance->z = ATOFF; NEXT + baseTuning->x = ATOFF; NEXT + baseTuning->y = ATOFF; NEXT + baseTuning->z = ATOFF; NEXT + adaptiveTuning->x = ATOFF; NEXT + adaptiveTuning->y = ATOFF; NEXT + adaptiveTuning->z = ATOFF; +} + +void Packet::parseFilterStartupGyroBias(vec3f* bias) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + bias->x = ATOFF; NEXT + bias->y = ATOFF; NEXT + bias->z = ATOFF; +} + +void Packet::parseMagnetometerCalibrationControl(uint8_t* hsiMode, uint8_t* hsiOutput, uint8_t* convergeRate) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *hsiMode = ATOU8; NEXT + *hsiOutput = ATOU8; NEXT + *convergeRate = ATOU8; +} + +void Packet::parseCalculatedMagnetometerCalibration(mat3f* c, vec3f* b) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + c->e00 = ATOFF; NEXT + c->e01 = ATOFF; NEXT + c->e02 = ATOFF; NEXT + c->e10 = ATOFF; NEXT + c->e11 = ATOFF; NEXT + c->e12 = ATOFF; NEXT + c->e20 = ATOFF; NEXT + c->e21 = ATOFF; NEXT + c->e22 = ATOFF; NEXT + b->x = ATOFF; NEXT + b->y = ATOFF; NEXT + b->z = ATOFF; +} + +void Packet::parseIndoorHeadingModeControl(float* maxRateError) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *maxRateError = ATOFF; NEXT +} + +void Packet::parseVelocityCompensationMeasurement(vec3f* velocity) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + velocity->x = ATOFF; NEXT + velocity->y = ATOFF; NEXT + velocity->z = ATOFF; +} + +void Packet::parseVelocityCompensationControl(uint8_t* mode, float* velocityTuning, float* rateTuning) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *mode = ATOU8; NEXT + *velocityTuning = ATOFF; NEXT + *rateTuning = ATOFF; +} + +void Packet::parseVelocityCompensationStatus(float* x, float* xDot, vec3f* accelOffset, vec3f* omega) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *x = ATOFF; NEXT + *xDot = ATOFF; NEXT + accelOffset->x = ATOFF; NEXT + accelOffset->y = ATOFF; NEXT + accelOffset->z = ATOFF; NEXT + omega->x = ATOFF; NEXT + omega->y = ATOFF; NEXT + omega->z = ATOFF; +} + +void Packet::parseImuMeasurements(vec3f* mag, vec3f* accel, vec3f* gyro, float* temp, float* pressure) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + mag->x = ATOFF; NEXT + mag->y = ATOFF; NEXT + mag->z = ATOFF; NEXT + accel->x = ATOFF; NEXT + accel->y = ATOFF; NEXT + accel->z = ATOFF; NEXT + gyro->x = ATOFF; NEXT + gyro->y = ATOFF; NEXT + gyro->z = ATOFF; NEXT + *temp = ATOFF; NEXT + *pressure = ATOFF; +} + +void Packet::parseGpsConfiguration(uint8_t* mode, uint8_t* ppsSource) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *mode = ATOU8; NEXT + *ppsSource = ATOU8; NEXT + NEXT + NEXT +} + +void Packet::parseGpsAntennaOffset(vec3f* position) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + position->x = ATOFF; NEXT + position->y = ATOFF; NEXT + position->z = ATOFF; +} + +void Packet::parseGpsSolutionLla(double* time, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, vec3d* lla, vec3f* nedVel, vec3f* nedAcc, float* speedAcc, float* timeAcc) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *time = ATOFD; NEXT + *week = ATOU16; NEXT + *gpsFix = ATOU8; NEXT + *numSats = ATOU8; NEXT + lla->x = ATOFD; NEXT + lla->y = ATOFD; NEXT + lla->z = ATOFD; NEXT + nedVel->x = ATOFF; NEXT + nedVel->y = ATOFF; NEXT + nedVel->z = ATOFF; NEXT + nedAcc->x = ATOFF; NEXT + nedAcc->y = ATOFF; NEXT + nedAcc->z = ATOFF; NEXT + *speedAcc = ATOFF; NEXT + *timeAcc = ATOFF; +} + +void Packet::parseGpsSolutionEcef(double* tow, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, vec3d* position, vec3f* velocity, vec3f* posAcc, float* speedAcc, float* timeAcc) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *tow = ATOFD; NEXT + *week = ATOU16; NEXT + *gpsFix = ATOU8; NEXT + *numSats = ATOU8; NEXT + position->x = ATOFD; NEXT + position->y = ATOFD; NEXT + position->z = ATOFD; NEXT + velocity->x = ATOFF; NEXT + velocity->y = ATOFF; NEXT + velocity->z = ATOFF; NEXT + posAcc->x = ATOFF; NEXT + posAcc->y = ATOFF; NEXT + posAcc->z = ATOFF; NEXT + *speedAcc = ATOFF; NEXT + *timeAcc = ATOFF; +} + +void Packet::parseInsSolutionLla(double* time, uint16_t* week, uint16_t* status, vec3f* yawPitchRoll, vec3d* position, vec3f* nedVel, float* attUncertainty, float* posUncertainty, float* velUncertainty) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *time = ATOFD; NEXT + *week = ATOU16; NEXT + *status = ATOU16X; NEXT + yawPitchRoll->x = ATOFF; NEXT + yawPitchRoll->y = ATOFF; NEXT + yawPitchRoll->z = ATOFF; NEXT + position->x = ATOFD; NEXT + position->y = ATOFD; NEXT + position->z = ATOFD; NEXT + nedVel->x = ATOFF; NEXT + nedVel->y = ATOFF; NEXT + nedVel->z = ATOFF; NEXT + *attUncertainty = ATOFF; NEXT + *posUncertainty = ATOFF; NEXT + *velUncertainty = ATOFF; +} + +void Packet::parseInsSolutionEcef(double* time, uint16_t* week, uint16_t* status, vec3f* yawPitchRoll, vec3d* position, vec3f* velocity, float* attUncertainty, float* posUncertainty, float* velUncertainty) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *time = ATOFD; NEXT + *week = ATOU16; NEXT + *status = ATOU16X; NEXT + yawPitchRoll->x = ATOFF; NEXT + yawPitchRoll->y = ATOFF; NEXT + yawPitchRoll->z = ATOFF; NEXT + position->x = ATOFD; NEXT + position->y = ATOFD; NEXT + position->z = ATOFD; NEXT + velocity->x = ATOFF; NEXT + velocity->y = ATOFF; NEXT + velocity->z = ATOFF; NEXT + *attUncertainty = ATOFF; NEXT + *posUncertainty = ATOFF; NEXT + *velUncertainty = ATOFF; +} + +void Packet::parseInsBasicConfiguration(uint8_t* scenario, uint8_t* ahrsAiding) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *scenario = ATOU8; NEXT + *ahrsAiding = ATOU8; NEXT + NEXT +} + +void Packet::parseInsBasicConfiguration(uint8_t* scenario, uint8_t* ahrsAiding, uint8_t* estBaseline) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *scenario = ATOU8; NEXT + *ahrsAiding = ATOU8; NEXT + *estBaseline = ATOU8; NEXT +} + +void Packet::parseInsAdvancedConfiguration(uint8_t* useMag, uint8_t* usePres, uint8_t* posAtt, uint8_t* velAtt, uint8_t* velBias, uint8_t* useFoam, uint8_t* gpsCovType, uint8_t* velCount, float* velInit, float* moveOrigin, float* gpsTimeout, float* deltaLimitPos, float* deltaLimitVel, float* minPosUncertainty, float* minVelUncertainty) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *useMag = ATOU8; NEXT + *usePres = ATOU8; NEXT + *posAtt = ATOU8; NEXT + *velAtt = ATOU8; NEXT + *velBias = ATOU8; NEXT + *useFoam = ATOU8; NEXT + *gpsCovType = ATOU8; NEXT + *velCount = ATOU8; NEXT + *velInit = ATOFF; NEXT + *moveOrigin = ATOFF; NEXT + *gpsTimeout = ATOFF; NEXT + *deltaLimitPos = ATOFF; NEXT + *deltaLimitVel = ATOFF; NEXT + *minPosUncertainty = ATOFF; NEXT + *minVelUncertainty = ATOFF; +} + +void Packet::parseInsStateLla(vec3f* yawPitchRoll, vec3d* position, vec3f* velocity, vec3f* accel, vec3f* angularRate) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + yawPitchRoll->x = ATOFF; NEXT + yawPitchRoll->y = ATOFF; NEXT + yawPitchRoll->z = ATOFF; NEXT + position->x = ATOFD; NEXT + position->y = ATOFD; NEXT + position->z = ATOFD; NEXT + velocity->x = ATOFF; NEXT + velocity->y = ATOFF; NEXT + velocity->z = ATOFF; NEXT + accel->x = ATOFF; NEXT + accel->y = ATOFF; NEXT + accel->z = ATOFF; NEXT + angularRate->x = ATOFF; NEXT + angularRate->y = ATOFF; NEXT + angularRate->z = ATOFF; +} + +void Packet::parseInsStateEcef(vec3f* yawPitchRoll, vec3d* position, vec3f* velocity, vec3f* accel, vec3f* angularRate) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + yawPitchRoll->x = ATOFF; NEXT + yawPitchRoll->y = ATOFF; NEXT + yawPitchRoll->z = ATOFF; NEXT + position->x = ATOFD; NEXT + position->y = ATOFD; NEXT + position->z = ATOFD; NEXT + velocity->x = ATOFF; NEXT + velocity->y = ATOFF; NEXT + velocity->z = ATOFF; NEXT + accel->x = ATOFF; NEXT + accel->y = ATOFF; NEXT + accel->z = ATOFF; NEXT + angularRate->x = ATOFF; NEXT + angularRate->y = ATOFF; NEXT + angularRate->z = ATOFF; +} + +void Packet::parseStartupFilterBiasEstimate(vec3f* gyroBias, vec3f* accelBias, float* pressureBias) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + gyroBias->x = ATOFF; NEXT + gyroBias->y = ATOFF; NEXT + gyroBias->z = ATOFF; NEXT + accelBias->x = ATOFF; NEXT + accelBias->y = ATOFF; NEXT + accelBias->z = ATOFF; NEXT + *pressureBias = ATOFF; +} + +void Packet::parseDeltaThetaAndDeltaVelocity(float* deltaTime, vec3f* deltaTheta, vec3f* deltaVelocity) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *deltaTime = ATOFF; NEXT + deltaTheta->x = ATOFF; NEXT + deltaTheta->y = ATOFF; NEXT + deltaTheta->z = ATOFF; NEXT + deltaVelocity->x = ATOFF; NEXT + deltaVelocity->y = ATOFF; NEXT + deltaVelocity->z = ATOFF; +} + +void Packet::parseDeltaThetaAndDeltaVelocityConfiguration(uint8_t* integrationFrame, uint8_t* gyroCompensation, uint8_t* accelCompensation) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *integrationFrame = ATOU8; NEXT + *gyroCompensation = ATOU8; NEXT + *accelCompensation = ATOU8; NEXT + NEXT +} + +void Packet::parseReferenceVectorConfiguration(uint8_t* useMagModel, uint8_t* useGravityModel, uint32_t* recalcThreshold, float* year, vec3d* position) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *useMagModel = ATOU8; NEXT + *useGravityModel = ATOU8; NEXT + NEXT + NEXT + *recalcThreshold = ATOU32; NEXT + *year = ATOFF; NEXT + position->x = ATOFD; NEXT + position->y = ATOFD; NEXT + position->z = ATOFD; +} + +void Packet::parseGyroCompensation(mat3f* c, vec3f* b) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + c->e00 = ATOFF; NEXT + c->e01 = ATOFF; NEXT + c->e02 = ATOFF; NEXT + c->e10 = ATOFF; NEXT + c->e11 = ATOFF; NEXT + c->e12 = ATOFF; NEXT + c->e20 = ATOFF; NEXT + c->e21 = ATOFF; NEXT + c->e22 = ATOFF; NEXT + b->x = ATOFF; NEXT + b->y = ATOFF; NEXT + b->z = ATOFF; +} + +void Packet::parseImuFilteringConfiguration(uint16_t* magWindowSize, uint16_t* accelWindowSize, uint16_t* gyroWindowSize, uint16_t* tempWindowSize, uint16_t* presWindowSize, uint8_t* magFilterMode, uint8_t* accelFilterMode, uint8_t* gyroFilterMode, uint8_t* tempFilterMode, uint8_t* presFilterMode) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *magWindowSize = ATOU16; NEXT + *accelWindowSize = ATOU16; NEXT + *gyroWindowSize = ATOU16; NEXT + *tempWindowSize = ATOU16; NEXT + *presWindowSize = ATOU16; NEXT + *magFilterMode = ATOU8; NEXT + *accelFilterMode = ATOU8; NEXT + *gyroFilterMode = ATOU8; NEXT + *tempFilterMode = ATOU8; NEXT + *presFilterMode = ATOU8; +} + +void Packet::parseGpsCompassBaseline(vec3f* position, vec3f* uncertainty) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + position->x = ATOFF; NEXT + position->y = ATOFF; NEXT + position->z = ATOFF; NEXT + uncertainty->x = ATOFF; NEXT + uncertainty->y = ATOFF; NEXT + uncertainty->z = ATOFF; +} + +void Packet::parseGpsCompassEstimatedBaseline(uint8_t* estBaselineUsed, uint16_t* numMeas, vec3f* position, vec3f* uncertainty) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *estBaselineUsed = ATOU8; NEXT + NEXT + *numMeas = ATOU16; NEXT + position->x = ATOFF; NEXT + position->y = ATOFF; NEXT + position->z = ATOFF; NEXT + uncertainty->x = ATOFF; NEXT + uncertainty->y = ATOFF; NEXT + uncertainty->z = ATOFF; +} + +void Packet::parseImuRateConfiguration(uint16_t* imuRate, uint16_t* navDivisor, float* filterTargetRate, float* filterMinRate) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + *imuRate = ATOU16; NEXT + *navDivisor = ATOU16; NEXT + *filterTargetRate = ATOFF; NEXT + *filterMinRate = ATOFF; +} + +void Packet::parseYawPitchRollTrueBodyAccelerationAndAngularRates(vec3f* yawPitchRoll, vec3f* bodyAccel, vec3f* gyro) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + yawPitchRoll->x = ATOFF; NEXT + yawPitchRoll->y = ATOFF; NEXT + yawPitchRoll->z = ATOFF; NEXT + bodyAccel->x = ATOFF; NEXT + bodyAccel->y = ATOFF; NEXT + bodyAccel->z = ATOFF; NEXT + gyro->x = ATOFF; NEXT + gyro->y = ATOFF; NEXT + gyro->z = ATOFF; +} + +void Packet::parseYawPitchRollTrueInertialAccelerationAndAngularRates(vec3f* yawPitchRoll, vec3f* inertialAccel, vec3f* gyro) +{ + size_t parseIndex; + + char *result = startAsciiResponsePacketParse(_data, parseIndex); + + yawPitchRoll->x = ATOFF; NEXT + yawPitchRoll->y = ATOFF; NEXT + yawPitchRoll->z = ATOFF; NEXT + inertialAccel->x = ATOFF; NEXT + inertialAccel->y = ATOFF; NEXT + inertialAccel->z = ATOFF; NEXT + gyro->x = ATOFF; NEXT + gyro->y = ATOFF; NEXT + gyro->z = ATOFF; +} + +} +} +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/protocol/uart/packetfinder.cpp b/ext/vnproglib-1.1.0.115/src/vn/protocol/uart/packetfinder.cpp new file mode 100755 index 0000000..a783e8a --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/protocol/uart/packetfinder.cpp @@ -0,0 +1,564 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/protocol/uart/packetfinder.h" + +#include +#include +#include +#include "vn/utilities.h" +#include "vn/data/error_detection.h" + +#if PYTHON + #include "vn/util/boostpython.h" + namespace bp = boost::python; +#endif + +using namespace std; +using namespace vn::xplat; +using namespace vn::data::integrity; + +namespace vn { +namespace protocol { +namespace uart { + +//char* vnstrtok(char* str, size_t& startIndex); + +struct PacketFinder::Impl +{ + static const size_t DefaultReceiveBufferSize = 512; + static const uint8_t AsciiStartChar = '$'; + static const uint8_t BinaryStartChar = 0xFA; + static const uint8_t AsciiEndChar1 = '\r'; + static const uint8_t AsciiEndChar2 = '\n'; + static const size_t MaximumSizeExpectedForBinaryPacket = 256; + static const size_t MaximumSizeForBinaryStartAndAllGroupData = 18; + static const size_t MaximumSizeForAsciiPacket = 256; + + struct AsciiTracker + { + bool currentlyBuildingAsciiPacket; + size_t possibleStartOfPacketIndex; + bool asciiEndChar1Found; + size_t runningDataIndexOfStart; + TimeStamp timeFound; + + AsciiTracker() : + currentlyBuildingAsciiPacket(false), + possibleStartOfPacketIndex(0), + asciiEndChar1Found(false), + runningDataIndexOfStart(0) + { } + + void reset() + { + currentlyBuildingAsciiPacket = false; + possibleStartOfPacketIndex = 0; + asciiEndChar1Found = false; + runningDataIndexOfStart = 0; + timeFound = TimeStamp(); + } + }; + + struct BinaryTracker + { + size_t possibleStartIndex; + bool groupsPresentFound; + uint8_t groupsPresent; + uint8_t numOfBytesRemainingToHaveAllGroupFields; + size_t numOfBytesRemainingForCompletePacket; + bool startFoundInProvidedDataBuffer; + size_t runningDataIndexOfStart; + TimeStamp timeFound; + + explicit BinaryTracker(size_t possibleStartIndex, size_t runningDataIndex, TimeStamp foundTime) : + possibleStartIndex(possibleStartIndex), + groupsPresentFound(false), + numOfBytesRemainingToHaveAllGroupFields(0), + numOfBytesRemainingForCompletePacket(0), + startFoundInProvidedDataBuffer(true), + runningDataIndexOfStart(runningDataIndex), + timeFound(foundTime) + { } + + bool operator==(const BinaryTracker &rhs) + { + return + possibleStartIndex == rhs.possibleStartIndex && + groupsPresentFound == rhs.groupsPresentFound && + groupsPresent == rhs.groupsPresent && + numOfBytesRemainingToHaveAllGroupFields == rhs.numOfBytesRemainingToHaveAllGroupFields && + numOfBytesRemainingForCompletePacket == rhs.numOfBytesRemainingForCompletePacket && + timeFound._sec == rhs.timeFound._sec && + timeFound._usec == rhs.timeFound._usec; + } + }; + + PacketFinder* _backReference; + uint8_t* _buffer; + const size_t _bufferSize; + size_t _bufferAppendLocation; + AsciiTracker _asciiOnDeck; + list _binaryOnDeck; // Collection of possible binary packets we are checking. + size_t _runningDataIndex; // Used for correlating raw data with where the packet was found for the end user. + void* _possiblePacketFoundUserData; + ValidPacketFoundHandler _possiblePacketFoundHandler; + #if PYTHON + /*boost::python::object* _pythonPacketFoundHandler;*/ + PyObject* _pythonPacketFoundHandler; + #endif + + explicit Impl(PacketFinder* backReference) : + _backReference(backReference), + _buffer(new uint8_t[DefaultReceiveBufferSize]), + _bufferSize(DefaultReceiveBufferSize), + _bufferAppendLocation(0), + _runningDataIndex(0), + _possiblePacketFoundUserData(NULL), + _possiblePacketFoundHandler(NULL) + #if PYTHON + , + _pythonPacketFoundHandler(NULL) + #endif + { } + + Impl(PacketFinder* backReference, size_t internalReceiveBufferSize) : + _backReference(backReference), + _buffer(new uint8_t[internalReceiveBufferSize]), + _bufferSize(internalReceiveBufferSize), + _bufferAppendLocation(0), + _runningDataIndex(0), + _possiblePacketFoundUserData(NULL), + _possiblePacketFoundHandler(NULL) + { } + + ~Impl() + { + delete [] _buffer; + } + + void resetTracking() + { + _asciiOnDeck.reset(); + _binaryOnDeck.clear(); + _bufferAppendLocation = 0; + } + + void dataReceived(uint8_t data[], size_t length, TimeStamp timestamp) + { + bool asciiStartFoundInProvidedBuffer = false; + + // Assume that since the _runningDataIndex is unsigned, any overflows + // will naturally go to zero, which is the behavior that we want. + for (size_t i = 0; i < length; i++, _runningDataIndex++) + { + if (data[i] == AsciiStartChar) + { + _asciiOnDeck.reset(); + _asciiOnDeck.currentlyBuildingAsciiPacket = true; + _asciiOnDeck.possibleStartOfPacketIndex = i; + _asciiOnDeck.runningDataIndexOfStart = _runningDataIndex; + _asciiOnDeck.timeFound = timestamp; + + asciiStartFoundInProvidedBuffer = true; + } + else if (_asciiOnDeck.currentlyBuildingAsciiPacket && data[i] == AsciiEndChar1) + { + _asciiOnDeck.asciiEndChar1Found = true; + } + else if (_asciiOnDeck.asciiEndChar1Found) + { + if (data[i] == AsciiEndChar2) + { + // We have a possible data packet. + size_t runningIndexOfPacketStart = _asciiOnDeck.runningDataIndexOfStart; + uint8_t* startOfAsciiPacket = NULL; + size_t packetLength = 0; + + if (asciiStartFoundInProvidedBuffer) + { + // All the packet was in this data buffer so we don't + // need to do any copying. + + startOfAsciiPacket = data + _asciiOnDeck.possibleStartOfPacketIndex; + packetLength = i - _asciiOnDeck.possibleStartOfPacketIndex + 1; + } + else + { + // The packet was split between the running data buffer + // the current data buffer. We need to copy the data + // over before further processing. + + if (_bufferAppendLocation + i < _bufferSize) + { + memcpy(_buffer + _bufferAppendLocation, data, i + 1); + + startOfAsciiPacket = _buffer + _asciiOnDeck.possibleStartOfPacketIndex; + packetLength = _bufferAppendLocation + i + 1 - _asciiOnDeck.possibleStartOfPacketIndex; + } + else + { + // We are about to overflow our buffer. Just fall + // through to reset tracking. + } + } + + Packet p(reinterpret_cast(startOfAsciiPacket), packetLength); + + if (p.isValid()) + dispatchPacket(p, runningIndexOfPacketStart, _asciiOnDeck.timeFound); + } + + // Either this is an invalid packet or was a packet that was processed. + if (_binaryOnDeck.empty()) + resetTracking(); + else + _asciiOnDeck.reset(); + asciiStartFoundInProvidedBuffer = false; + } + else if (i + 1 > MaximumSizeForAsciiPacket) + { + // This must not be a valid ASCII packet. + if (_binaryOnDeck.empty()) + { + resetTracking(); + } + else + { + _asciiOnDeck.reset(); + asciiStartFoundInProvidedBuffer = false; + } + } + + // Update all of our binary packets on deck. + queue invalidPackets; + for (list::iterator it = _binaryOnDeck.begin(); it != _binaryOnDeck.end(); ++it) + { + BinaryTracker &ez = (*it); + + if (!ez.groupsPresentFound) + { + // This byte must be the groups present. + ez.groupsPresentFound = true; + ez.groupsPresent = data[i]; + ez.numOfBytesRemainingToHaveAllGroupFields = 2 * countSetBits(data[i]); + + continue; + } + + if (ez.numOfBytesRemainingToHaveAllGroupFields != 0) + { + // We found another byte belonging to this possible binary packet. + ez.numOfBytesRemainingToHaveAllGroupFields--; + + if (ez.numOfBytesRemainingToHaveAllGroupFields == 0) + { + // We have all of the group fields now. + size_t remainingBytesForCompletePacket; + if (ez.startFoundInProvidedDataBuffer) + { + size_t headerLength = i - ez.possibleStartIndex + 1; + remainingBytesForCompletePacket = Packet::computeBinaryPacketLength(reinterpret_cast(data) + ez.possibleStartIndex) - headerLength; + } + else + { + // Not all of the packet's group is inside the caller's provided buffer. + + // Temporarily copy the rest of the packet to the receive buffer + // for computing the size of the packet. + + size_t numOfBytesToCopyIntoReceiveBuffer = i + 1; + size_t headerLength = _bufferAppendLocation - ez.possibleStartIndex + numOfBytesToCopyIntoReceiveBuffer; + + if (_bufferAppendLocation + numOfBytesToCopyIntoReceiveBuffer < _bufferSize) + { + std::memcpy(_buffer + _bufferAppendLocation, data, numOfBytesToCopyIntoReceiveBuffer); + + remainingBytesForCompletePacket = Packet::computeBinaryPacketLength(reinterpret_cast(_buffer) + ez.possibleStartIndex) - headerLength; + } + else + { + // About to overrun our receive buffer! + invalidPackets.push(ez); + + // TODO: Should we just go ahead and clear the ASCII tracker + // and buffer append location? + + continue; + } + } + + if (remainingBytesForCompletePacket > MaximumSizeExpectedForBinaryPacket) + { + // Must be a bad possible binary packet. + invalidPackets.push(ez); + } + else + { + ez.numOfBytesRemainingForCompletePacket = remainingBytesForCompletePacket; + } + } + + continue; + } + + // We are currently collecting data for our packet. + + ez.numOfBytesRemainingForCompletePacket--; + + if (ez.numOfBytesRemainingForCompletePacket == 0) + { + // We have a possible binary packet! + + uint8_t* packetStart; + size_t packetLength; + + if (ez.startFoundInProvidedDataBuffer) + { + // The binary packet exists completely in the user's provided buffer. + packetStart = data + ez.possibleStartIndex; + packetLength = i - ez.possibleStartIndex + 1; + } + else + { + // The packet is split between our receive buffer and the user's buffer. + size_t numOfBytesToCopyIntoReceiveBuffer = i + 1; + + if (_bufferAppendLocation + numOfBytesToCopyIntoReceiveBuffer < _bufferSize) + { + std::memcpy(_buffer + _bufferAppendLocation, data, numOfBytesToCopyIntoReceiveBuffer); + + packetStart = _buffer + ez.possibleStartIndex; + packetLength = _bufferAppendLocation - ez.possibleStartIndex + i + 1; + } + else + { + // About to overrun our receive buffer! + invalidPackets.push(ez); + + continue; + } + } + + Packet p(reinterpret_cast(packetStart), packetLength); + + if (!p.isValid()) + { + // Invalid packet! + invalidPackets.push(ez); + } + else + { + // We have a valid binary packet!!!. + size_t packetStartRunningIndex = (ez).runningDataIndexOfStart; + invalidPackets = queue(); + resetTracking(); + + dispatchPacket(p, packetStartRunningIndex, ez.timeFound); + + break; + } + } + } + + // Remove any invalid packets. + while (!invalidPackets.empty()) + { + _binaryOnDeck.remove(invalidPackets.front()); + invalidPackets.pop(); + } + + if (_binaryOnDeck.empty() && !_asciiOnDeck.currentlyBuildingAsciiPacket) + { + _bufferAppendLocation = 0; + } + + if (data[i] == BinaryStartChar) + { + // Possible start of a binary packet. + _binaryOnDeck.push_back(BinaryTracker(i, _runningDataIndex, timestamp)); + } + } + + if (_binaryOnDeck.empty() && !_asciiOnDeck.currentlyBuildingAsciiPacket) + // No data to copy over. + return; + + // Perform any data copying to our receive buffer. + + size_t dataIndexToStartCopyingFrom = 0; + bool binaryDataToCopyOver = false; + size_t binaryDataMoveOverIndexAdjustment = 0; + + if (!_binaryOnDeck.empty()) + { + binaryDataToCopyOver = true; + + if (_binaryOnDeck.front().startFoundInProvidedDataBuffer) + { + dataIndexToStartCopyingFrom = _binaryOnDeck.front().possibleStartIndex; + binaryDataMoveOverIndexAdjustment = dataIndexToStartCopyingFrom; + } + } + + if (_asciiOnDeck.currentlyBuildingAsciiPacket && asciiStartFoundInProvidedBuffer) + { + if (_asciiOnDeck.possibleStartOfPacketIndex < dataIndexToStartCopyingFrom) + { + binaryDataMoveOverIndexAdjustment -= binaryDataMoveOverIndexAdjustment - _asciiOnDeck.possibleStartOfPacketIndex; + dataIndexToStartCopyingFrom = _asciiOnDeck.possibleStartOfPacketIndex; + } + else if (!binaryDataToCopyOver) + { + dataIndexToStartCopyingFrom = _asciiOnDeck.possibleStartOfPacketIndex; + } + + // Adjust our ASCII index to be based on the recieve buffer. + _asciiOnDeck.possibleStartOfPacketIndex = _bufferAppendLocation + _asciiOnDeck.possibleStartOfPacketIndex - dataIndexToStartCopyingFrom; + } + + // Adjust any binary packet indexes we are currently building. + for (list::iterator it = _binaryOnDeck.begin(); it != _binaryOnDeck.end(); ++it) + { + if ((*it).startFoundInProvidedDataBuffer) + { + (*it).startFoundInProvidedDataBuffer = false; + (*it).possibleStartIndex = (*it).possibleStartIndex - binaryDataMoveOverIndexAdjustment + _bufferAppendLocation; + } + } + + if (_bufferAppendLocation + length - dataIndexToStartCopyingFrom < _bufferSize) + { + // Safe to copy over the data. + + size_t numOfBytesToCopyOver = length - dataIndexToStartCopyingFrom; + uint8_t *copyFromStart = data + dataIndexToStartCopyingFrom; + + std::memcpy(_buffer + _bufferAppendLocation, copyFromStart, numOfBytesToCopyOver); + _bufferAppendLocation += numOfBytesToCopyOver; + } + else + { + // We are about to overflow our buffer. + resetTracking(); + } + } + + void dispatchPacket(Packet &packet, size_t runningDataIndexAtPacketStart, TimeStamp timestamp) + { + if (_possiblePacketFoundHandler != NULL) + { + _possiblePacketFoundHandler(_possiblePacketFoundUserData, packet, runningDataIndexAtPacketStart, timestamp); + } + + #if PYTHON + //using namespace std; + //cout << "Made Here" << endl; + if (_pythonPacketFoundHandler != NULL) + { + //#vector pRawData(readBuffer, readBuffer + numOfBytesRead); + + //python::AcquireGIL scopedLock; + + //_pythonPacketFoundHandler. + //_pythonPacketFoundHandler(); + boost::python::call(_pythonPacketFoundHandler, packet); + + //boost::python::call(pi->_rawDataReceivedHandlerPython, pRawData, pi->_dataRunningIndex); + } + #endif + } +}; + +#if defined(_MSC_VER) && _MSC_VER <= 1600 + #pragma warning(push) + // Disable VS2010 warning for 'this' used in base member initializer list. + #pragma warning(disable:4355) +#endif + +PacketFinder::PacketFinder() : + _pi(new Impl(this)) +{ +} + +PacketFinder::PacketFinder(size_t internalReceiveBufferSize) : + _pi(new Impl(this, internalReceiveBufferSize)) +{ +} + +#if defined(_MSC_VER) && _MSC_VER <= 1600 + #pragma warning(pop) +#endif + +PacketFinder::~PacketFinder() +{ + delete _pi; +} + +void PacketFinder::processReceivedData(char data[], size_t length) +{ + TimeStamp placeholder; + + processReceivedData(data, length, placeholder); +} + +void PacketFinder::processReceivedData(char data[], size_t length, TimeStamp timestamp) +{ + _pi->dataReceived(reinterpret_cast(data), length, timestamp); +} + +#if PYTHON + +void PacketFinder::processReceivedData(boost::python::list data) +{ + //size_t len = boost::python::len(data); + //unique cppData = make_unique(len); + + //for (auto i = 0; i < len; i++) + // cppData[i] = boost::python::extract(data[i]); + + //processReceivedData(cppData.get(), len); +} + +#endif + +void PacketFinder::registerPossiblePacketFoundHandler(void* userData, ValidPacketFoundHandler handler) +{ + if (_pi->_possiblePacketFoundHandler != NULL) + throw invalid_operation(); + + _pi->_possiblePacketFoundHandler = handler; + _pi->_possiblePacketFoundUserData = userData; +} + +void PacketFinder::unregisterPossiblePacketFoundHandler() +{ + if (_pi->_possiblePacketFoundHandler == NULL) + throw invalid_operation(); + + _pi->_possiblePacketFoundHandler = NULL; + _pi->_possiblePacketFoundUserData = NULL; +} + +#if PYTHON + +//void PacketFinder::register_packet_found_handler(boost::python::object* callable) +boost::python::object* PacketFinder::register_packet_found_handler(PyObject* callable/*boost::python::object* callable*/) +{ + _pi->_pythonPacketFoundHandler = callable; + callable->ob_refcnt++; + //_pi->_pythonPacketFoundHandler = callable; + //callable->ptr()->ob_refcnt++; + //callable->ptr(); + //_pi->_rawDataReceivedHandlerPython = callable; + //callable->ob_refcnt++; + + return NULL; + + //return Py_None; +} + +#endif + +} +} +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/protocol/uart/python.cpp b/ext/vnproglib-1.1.0.115/src/vn/protocol/uart/python.cpp new file mode 100755 index 0000000..b73a7ef --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/protocol/uart/python.cpp @@ -0,0 +1,1673 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "boost/python.hpp" + +#include "vn/protocol/uart/packet.h" +#include "vn/protocol/uart/packetfinder.h" + +using namespace std; +using namespace boost::python; +using namespace vn::math; +using namespace vn::protocol::uart; + +// This group of identity_ functions allow converting a number in Python into +// the corresponding enum which is allow printing of the enum as a textual +// string representation instead of just returning a number. +AsciiAsync identity_(AsciiAsync aa) { return aa; } +BinaryGroup identity_(BinaryGroup bg) { return bg; } +CommonGroup identity_(CommonGroup cg) { return cg; } +TimeGroup identity_(TimeGroup tg) { return tg; } +ImuGroup identity_(ImuGroup ig) { return ig; } +GpsGroup identity_(GpsGroup gg) { return gg; } +AttitudeGroup identity_(AttitudeGroup cg) { return cg; } +InsGroup identity_(InsGroup ig) { return ig; } + +//boost::shared_ptr packetConstructor(object const &p) +/*boost::shared_ptr packetConstructor(std::string const &p) +{ + return boost::make_shared() + cout << "It's working!!!" << flush << endl; + return boost::make_shared(); +}*/ + +#if PYTHON_OLD + +// This used to be a Python compatible constructor for the Packet class. +// However, I am in the process of removing all Python related code from +// the raw C++ library class and moving this fuctionality into the Python +// wrappers themselves. -Paul + +Packet::Packet(Type type, boost::python::list packet_data) : +_isPacketDataMine(true), +_length(len(packet_data)), +_data(new char[_length]), +_curExtractLoc(0) +{ + for (auto i = 0u; i < _length; i++) + _data[i] = bp::extract(packet_data[i]); +} + +#endif + +vec3f packetParseVNYPR(Packet& packet) +{ + vec3f holder; + packet.parseVNYPR(&holder); + + return holder; +} + +bool isCompatible(Packet& packet, + uint32_t commonGroup, + uint32_t timeGroup, + uint32_t imuGroup, + uint32_t gpsGroup, + uint32_t attitudeGroup, + uint32_t insGroup) +{ + return packet.isCompatible(static_cast(commonGroup), + static_cast(timeGroup), + static_cast(imuGroup), + static_cast(gpsGroup), + static_cast(attitudeGroup), + static_cast(insGroup)); +} + +string packetParseUserTag(Packet& packet) +{ + char holder[21]; + packet.parseUserTag(holder); + + return string(holder); +} + +string packetParseModelNumber(Packet& packet) +{ + char holder[25]; + packet.parseModelNumber(holder); + + return string(holder); +} + +uint32_t packetParseHardwareRevision(Packet& packet) +{ + uint32_t holder; + packet.parseHardwareRevision(&holder); + + return holder; +} + +uint32_t packetParseSerialNumber(Packet& packet) +{ + uint32_t holder; + packet.parseSerialNumber(&holder); + + return holder; +} + +string packetParseFirmwareVersion(Packet& packet) +{ + char holder[16]; + packet.parseFirmwareVersion(holder); + + return string(holder); +} + +uint32_t packetParseSerialBaudRate(Packet& packet) +{ + uint32_t holder; + packet.parseSerialBaudRate(&holder); + + return holder; +} + +uint32_t packetParseAsyncDataOutputType(Packet& packet) +{ + uint32_t holder; + packet.parseAsyncDataOutputType(&holder); + + return holder; +} + +uint32_t packetParseAsyncDataOutputFrequency(Packet& packet) +{ + uint32_t holder; + packet.parseAsyncDataOutputFrequency(&holder); + + return holder; +} + +vec3f packetParseYawPitchRoll(Packet& packet) +{ + vec3f holder; + packet.parseYawPitchRoll(&holder); + + return holder; +} + +vec4f packetParseAttitudeQuaternion(Packet& packet) +{ + vec4f holder; + packet.parseAttitudeQuaternion(&holder); + + return holder; +} + +list packetParseQuaternionMagneticAccelerationAndAngularRates(Packet& packet) +{ + list l; + + vec4f quat; + vec3f mag; + vec3f accel; + vec3f gyro; + + packet.parseQuaternionMagneticAccelerationAndAngularRates( + &quat, + &mag, + &accel, + &gyro); + + l.append(quat); + l.append(mag); + l.append(accel); + l.append(gyro); + + return l; +} + +vec3f packetParseMagneticMeasurements(Packet& packet) +{ + vec3f holder; + packet.parseMagneticMeasurements(&holder); + + return holder; +} + +vec3f packetParseAccelerationMeasurements(Packet& packet) +{ + vec3f holder; + packet.parseAccelerationMeasurements(&holder); + + return holder; +} + +vec3f packetParseAngularRateMeasurements(Packet& packet) +{ + vec3f holder; + packet.parseAngularRateMeasurements(&holder); + + return holder; +} + +list packetParseMagneticAccelerationAndAngularRates(Packet& packet) +{ + list l; + + vec3f mag; + vec3f accel; + vec3f gyro; + + packet.parseMagneticAccelerationAndAngularRates( + &mag, + &accel, + &gyro); + + l.append(mag); + l.append(accel); + l.append(gyro); + + return l; +} + +list packetParseMagneticAndGravityReferenceVectors(Packet& packet) +{ + list l; + + vec3f magRef; + vec3f accRef; + + packet.parseMagneticAndGravityReferenceVectors( + &magRef, + &accRef); + + l.append(magRef); + l.append(accRef); + + return l; +} + +list packetParseFilterMeasurementsVarianceParameters(Packet& packet) +{ + list l; + + float angularWalkVariance; + vec3f angularRateVariance; + vec3f magneticVariance; + vec3f accelerationVariance; + + packet.parseFilterMeasurementsVarianceParameters( + &angularWalkVariance, + &angularRateVariance, + &magneticVariance, + &accelerationVariance); + + l.append(angularWalkVariance); + l.append(angularRateVariance); + l.append(magneticVariance); + l.append(accelerationVariance); + + return l; +} + +list packetParseMagnetometerCompensation(Packet& packet) +{ + list l; + + mat3f c; + vec3f b; + + packet.parseMagnetometerCompensation( + &c, + &b); + + l.append(c); + l.append(b); + + return l; +} + +list packetParseFilterActiveTuningParameters(Packet& packet) +{ + list l; + + float magneticDisturbanceGain; + float accelerationDisturbanceGain; + float magneticDisturbanceMemory; + float accelerationDisturbanceMemory; + + packet.parseFilterActiveTuningParameters( + &magneticDisturbanceGain, + &accelerationDisturbanceGain, + &magneticDisturbanceMemory, + &accelerationDisturbanceMemory); + + l.append(magneticDisturbanceGain); + l.append(accelerationDisturbanceGain); + l.append(magneticDisturbanceMemory); + l.append(accelerationDisturbanceMemory); + + return l; +} + +list packetParseAccelerationCompensation(Packet& packet) +{ + list l; + + mat3f c; + vec3f b; + + packet.parseAccelerationCompensation( + &c, + &b); + + l.append(c); + l.append(b); + + return l; +} + +mat3f packetParseReferenceFrameRotation(Packet& packet) +{ + mat3f holder; + packet.parseReferenceFrameRotation(&holder); + + return holder; +} + +list packetParseYawPitchRollMagneticAccelerationAndAngularRates(Packet& packet) +{ + list l; + + vec3f yawPitchRoll; + vec3f mag; + vec3f accel; + vec3f gyro; + + packet.parseYawPitchRollMagneticAccelerationAndAngularRates( + &yawPitchRoll, + &mag, + &accel, + &gyro); + + l.append(yawPitchRoll); + l.append(mag); + l.append(accel); + l.append(gyro); + + return l; +} + +list packetParseCommunicationProtocolControl(Packet& packet) +{ + list l; + + uint8_t serialCount; + uint8_t serialStatus; + uint8_t spiCount; + uint8_t spiStatus; + uint8_t serialChecksum; + uint8_t spiChecksum; + uint8_t errorMode; + + packet.parseCommunicationProtocolControl( + &serialCount, + &serialStatus, + &spiCount, + &spiStatus, + &serialChecksum, + &spiChecksum, + &errorMode); + + l.append(serialCount); + l.append(serialStatus); + l.append(spiCount); + l.append(spiStatus); + l.append(serialChecksum); + l.append(spiChecksum); + l.append(errorMode); + + return l; +} + +list packetParseSynchronizationControl(Packet& packet) +{ + list l; + + uint8_t syncInMode; + uint8_t syncInEdge; + uint16_t syncInSkipFactor; + uint8_t syncOutMode; + uint8_t syncOutPolarity; + uint16_t syncOutSkipFactor; + uint32_t syncOutPulseWidth; + + packet.parseSynchronizationControl( + &syncInMode, + &syncInEdge, + &syncInSkipFactor, + &syncOutMode, + &syncOutPolarity, + &syncOutSkipFactor, + &syncOutPulseWidth); + + l.append(syncInMode); + l.append(syncInEdge); + l.append(syncInSkipFactor); + l.append(syncOutMode); + l.append(syncOutPolarity); + l.append(syncOutSkipFactor); + l.append(syncOutPulseWidth); + + return l; +} + +list packetParseSynchronizationStatus(Packet& packet) +{ + list l; + + uint32_t syncInCount; + uint32_t syncInTime; + uint32_t syncOutCount; + + packet.parseSynchronizationStatus( + &syncInCount, + &syncInTime, + &syncOutCount); + + l.append(syncInCount); + l.append(syncInTime); + l.append(syncOutCount); + + return l; +} + +list packetParseFilterBasicControl(Packet& packet) +{ + list l; + + uint8_t magMode; + uint8_t extMagMode; + uint8_t extAccMode; + uint8_t extGyroMode; + vec3f gyroLimit; + + packet.parseFilterBasicControl( + &magMode, + &extMagMode, + &extAccMode, + &extGyroMode, + &gyroLimit); + + l.append(magMode); + l.append(extMagMode); + l.append(extAccMode); + l.append(extGyroMode); + l.append(gyroLimit); + + return l; +} + +list packetParseVpeBasicControl(Packet& packet) +{ + list l; + + uint8_t enable; + uint8_t headingMode; + uint8_t filteringMode; + uint8_t tuningMode; + + packet.parseVpeBasicControl( + &enable, + &headingMode, + &filteringMode, + &tuningMode); + + l.append(enable); + l.append(headingMode); + l.append(filteringMode); + l.append(tuningMode); + + return l; +} + +list packetParseVpeMagnetometerBasicTuning(Packet& packet) +{ + list l; + + vec3f baseTuning; + vec3f adaptiveTuning; + vec3f adaptiveFiltering; + + packet.parseVpeMagnetometerBasicTuning( + &baseTuning, + &adaptiveTuning, + &adaptiveFiltering); + + l.append(baseTuning); + l.append(adaptiveTuning); + l.append(adaptiveFiltering); + + return l; +} + +list packetParseVpeMagnetometerAdvancedTuning(Packet& packet) +{ + list l; + + vec3f minFiltering; + vec3f maxFiltering; + float maxAdaptRate; + float disturbanceWindow; + float maxTuning; + + packet.parseVpeMagnetometerAdvancedTuning( + &minFiltering, + &maxFiltering, + &maxAdaptRate, + &disturbanceWindow, + &maxTuning); + + l.append(minFiltering); + l.append(maxFiltering); + l.append(maxAdaptRate); + l.append(disturbanceWindow); + l.append(maxTuning); + + return l; +} + +list packetParseVpeAccelerometerBasicTuning(Packet& packet) +{ + list l; + + vec3f baseTuning; + vec3f adaptiveTuning; + vec3f adaptiveFiltering; + + packet.parseVpeAccelerometerBasicTuning( + &baseTuning, + &adaptiveTuning, + &adaptiveFiltering); + + l.append(baseTuning); + l.append(adaptiveTuning); + l.append(adaptiveFiltering); + + return l; +} + +list packetParseVpeAccelerometerAdvancedTuning(Packet& packet) +{ + list l; + + vec3f minFiltering; + vec3f maxFiltering; + float maxAdaptRate; + float disturbanceWindow; + float maxTuning; + + packet.parseVpeAccelerometerAdvancedTuning( + &minFiltering, + &maxFiltering, + &maxAdaptRate, + &disturbanceWindow, + &maxTuning); + + l.append(minFiltering); + l.append(maxFiltering); + l.append(maxAdaptRate); + l.append(disturbanceWindow); + l.append(maxTuning); + + return l; +} + +list packetParseVpeGryoBasicTuning(Packet& packet) +{ + list l; + + vec3f angularWalkVariance; + vec3f baseTuning; + vec3f adaptiveTuning; + + packet.parseVpeGryoBasicTuning( + &angularWalkVariance, + &baseTuning, + &adaptiveTuning); + + l.append(angularWalkVariance); + l.append(baseTuning); + l.append(adaptiveTuning); + + return l; +} + +vec3f packetParseFilterStartupGyroBias(Packet& packet) +{ + vec3f holder; + packet.parseFilterStartupGyroBias(&holder); + + return holder; +} + +list packetParseMagnetometerCalibrationControl(Packet& packet) +{ + list l; + + uint8_t hsiMode; + uint8_t hsiOutput; + uint8_t convergeRate; + + packet.parseMagnetometerCalibrationControl( + &hsiMode, + &hsiOutput, + &convergeRate); + + l.append(hsiMode); + l.append(hsiOutput); + l.append(convergeRate); + + return l; +} + +list packetParseCalculatedMagnetometerCalibration(Packet& packet) +{ + list l; + + mat3f c; + vec3f b; + + packet.parseCalculatedMagnetometerCalibration( + &c, + &b); + + l.append(c); + l.append(b); + + return l; +} + +float packetParseIndoorHeadingModeControl(Packet& packet) +{ + float holder; + packet.parseIndoorHeadingModeControl(&holder); + + return holder; +} + +vec3f packetParseVelocityCompensationMeasurement(Packet& packet) +{ + vec3f holder; + packet.parseVelocityCompensationMeasurement(&holder); + + return holder; +} + +list packetParseVelocityCompensationControl(Packet& packet) +{ + list l; + + uint8_t mode; + float velocityTuning; + float rateTuning; + + packet.parseVelocityCompensationControl( + &mode, + &velocityTuning, + &rateTuning); + + l.append(mode); + l.append(velocityTuning); + l.append(rateTuning); + + return l; +} + +list packetParseVelocityCompensationStatus(Packet& packet) +{ + list l; + + float x; + float xDot; + vec3f accelOffset; + vec3f omega; + + packet.parseVelocityCompensationStatus( + &x, + &xDot, + &accelOffset, + &omega); + + l.append(x); + l.append(xDot); + l.append(accelOffset); + l.append(omega); + + return l; +} + +list packetParseImuMeasurements(Packet& packet) +{ + list l; + + vec3f mag; + vec3f accel; + vec3f gyro; + float temp; + float pressure; + + packet.parseImuMeasurements( + &mag, + &accel, + &gyro, + &temp, + &pressure); + + l.append(mag); + l.append(accel); + l.append(gyro); + l.append(temp); + l.append(pressure); + + return l; +} + +list packetParseGpsConfiguration(Packet& packet) +{ + list l; + + uint8_t mode; + uint8_t ppsSource; + + packet.parseGpsConfiguration( + &mode, + &ppsSource); + + l.append(mode); + l.append(ppsSource); + + return l; +} + +vec3f packetParseGpsAntennaOffset(Packet& packet) +{ + vec3f holder; + packet.parseGpsAntennaOffset(&holder); + + return holder; +} + +list packetParseGpsSolutionLla(Packet& packet) +{ + list l; + + double time; + uint16_t week; + uint8_t gpsFix; + uint8_t numSats; + vec3d lla; + vec3f nedVel; + vec3f nedAcc; + float speedAcc; + float timeAcc; + + packet.parseGpsSolutionLla( + &time, + &week, + &gpsFix, + &numSats, + &lla, + &nedVel, + &nedAcc, + &speedAcc, + &timeAcc); + + l.append(time); + l.append(week); + l.append(gpsFix); + l.append(numSats); + l.append(lla); + l.append(nedVel); + l.append(nedAcc); + l.append(speedAcc); + l.append(timeAcc); + + return l; +} + +list packetParseGpsSolutionEcef(Packet& packet) +{ + list l; + + double tow; + uint16_t week; + uint8_t gpsFix; + uint8_t numSats; + vec3d position; + vec3f velocity; + vec3f posAcc; + float speedAcc; + float timeAcc; + + packet.parseGpsSolutionEcef( + &tow, + &week, + &gpsFix, + &numSats, + &position, + &velocity, + &posAcc, + &speedAcc, + &timeAcc); + + l.append(tow); + l.append(week); + l.append(gpsFix); + l.append(numSats); + l.append(position); + l.append(velocity); + l.append(posAcc); + l.append(speedAcc); + l.append(timeAcc); + + return l; +} + +list packetParseInsSolutionLla(Packet& packet) +{ + list l; + + double time; + uint16_t week; + uint16_t status; + vec3f yawPitchRoll; + vec3d position; + vec3f nedVel; + float attUncertainty; + float posUncertainty; + float velUncertainty; + + packet.parseInsSolutionLla( + &time, + &week, + &status, + &yawPitchRoll, + &position, + &nedVel, + &attUncertainty, + &posUncertainty, + &velUncertainty); + + l.append(time); + l.append(week); + l.append(status); + l.append(yawPitchRoll); + l.append(position); + l.append(nedVel); + l.append(attUncertainty); + l.append(posUncertainty); + l.append(velUncertainty); + + return l; +} + +list packetParseInsSolutionEcef(Packet& packet) +{ + list l; + + double time; + uint16_t week; + uint16_t status; + vec3f yawPitchRoll; + vec3d position; + vec3f velocity; + float attUncertainty; + float posUncertainty; + float velUncertainty; + + packet.parseInsSolutionEcef( + &time, + &week, + &status, + &yawPitchRoll, + &position, + &velocity, + &attUncertainty, + &posUncertainty, + &velUncertainty); + + l.append(time); + l.append(week); + l.append(status); + l.append(yawPitchRoll); + l.append(position); + l.append(velocity); + l.append(attUncertainty); + l.append(posUncertainty); + l.append(velUncertainty); + + return l; +} + +list packetParseInsAdvancedConfiguration(Packet& packet) +{ + list l; + + uint8_t useMag; + uint8_t usePres; + uint8_t posAtt; + uint8_t velAtt; + uint8_t velBias; + uint8_t useFoam; + uint8_t gpsCovType; + uint8_t velCount; + float velInit; + float moveOrigin; + float gpsTimeout; + float deltaLimitPos; + float deltaLimitVel; + float minPosUncertainty; + float minVelUncertainty; + + packet.parseInsAdvancedConfiguration( + &useMag, + &usePres, + &posAtt, + &velAtt, + &velBias, + &useFoam, + &gpsCovType, + &velCount, + &velInit, + &moveOrigin, + &gpsTimeout, + &deltaLimitPos, + &deltaLimitVel, + &minPosUncertainty, + &minVelUncertainty); + + l.append(useMag); + l.append(usePres); + l.append(posAtt); + l.append(velAtt); + l.append(velBias); + l.append(useFoam); + l.append(gpsCovType); + l.append(velCount); + l.append(velInit); + l.append(moveOrigin); + l.append(gpsTimeout); + l.append(deltaLimitPos); + l.append(deltaLimitVel); + l.append(minPosUncertainty); + l.append(minVelUncertainty); + + return l; +} + +list packetParseInsStateLla(Packet& packet) +{ + list l; + + vec3f yawPitchRoll; + vec3d position; + vec3f velocity; + vec3f accel; + vec3f angularRate; + + packet.parseInsStateLla( + &yawPitchRoll, + &position, + &velocity, + &accel, + &angularRate); + + l.append(yawPitchRoll); + l.append(position); + l.append(velocity); + l.append(accel); + l.append(angularRate); + + return l; +} + +list packetParseInsStateEcef(Packet& packet) +{ + list l; + + vec3f yawPitchRoll; + vec3d position; + vec3f velocity; + vec3f accel; + vec3f angularRate; + + packet.parseInsStateEcef( + &yawPitchRoll, + &position, + &velocity, + &accel, + &angularRate); + + l.append(yawPitchRoll); + l.append(position); + l.append(velocity); + l.append(accel); + l.append(angularRate); + + return l; +} + +list packetParseStartupFilterBiasEstimate(Packet& packet) +{ + list l; + + vec3f gyroBias; + vec3f accelBias; + float pressureBias; + + packet.parseStartupFilterBiasEstimate( + &gyroBias, + &accelBias, + &pressureBias); + + l.append(gyroBias); + l.append(accelBias); + l.append(pressureBias); + + return l; +} + +list packetParseDeltaThetaAndDeltaVelocity(Packet& packet) +{ + list l; + + float deltaTime; + vec3f deltaTheta; + vec3f deltaVelocity; + + packet.parseDeltaThetaAndDeltaVelocity( + &deltaTime, + &deltaTheta, + &deltaVelocity); + + l.append(deltaTime); + l.append(deltaTheta); + l.append(deltaVelocity); + + return l; +} + +list packetParseDeltaThetaAndDeltaVelocityConfiguration(Packet& packet) +{ + list l; + + uint8_t integrationFrame; + uint8_t gyroCompensation; + uint8_t accelCompensation; + + packet.parseDeltaThetaAndDeltaVelocityConfiguration( + &integrationFrame, + &gyroCompensation, + &accelCompensation); + + l.append(integrationFrame); + l.append(gyroCompensation); + l.append(accelCompensation); + + return l; +} + +list packetParseReferenceVectorConfiguration(Packet& packet) +{ + list l; + + uint8_t useMagModel; + uint8_t useGravityModel; + uint32_t recalcThreshold; + float year; + vec3d position; + + packet.parseReferenceVectorConfiguration( + &useMagModel, + &useGravityModel, + &recalcThreshold, + &year, + &position); + + l.append(useMagModel); + l.append(useGravityModel); + l.append(recalcThreshold); + l.append(year); + l.append(position); + + return l; +} + +list packetParseGyroCompensation(Packet& packet) +{ + list l; + + mat3f c; + vec3f b; + + packet.parseGyroCompensation( + &c, + &b); + + l.append(c); + l.append(b); + + return l; +} + +list packetParseImuFilteringConfiguration(Packet& packet) +{ + list l; + + uint16_t magWindowSize; + uint16_t accelWindowSize; + uint16_t gyroWindowSize; + uint16_t tempWindowSize; + uint16_t presWindowSize; + uint8_t magFilterMode; + uint8_t accelFilterMode; + uint8_t gyroFilterMode; + uint8_t tempFilterMode; + uint8_t presFilterMode; + + packet.parseImuFilteringConfiguration( + &magWindowSize, + &accelWindowSize, + &gyroWindowSize, + &tempWindowSize, + &presWindowSize, + &magFilterMode, + &accelFilterMode, + &gyroFilterMode, + &tempFilterMode, + &presFilterMode); + + l.append(magWindowSize); + l.append(accelWindowSize); + l.append(gyroWindowSize); + l.append(tempWindowSize); + l.append(presWindowSize); + l.append(magFilterMode); + l.append(accelFilterMode); + l.append(gyroFilterMode); + l.append(tempFilterMode); + l.append(presFilterMode); + + return l; +} + +list packetParseGpsCompassBaseline(Packet& packet) +{ + list l; + + vec3f position; + vec3f uncertainty; + + packet.parseGpsCompassBaseline( + &position, + &uncertainty); + + l.append(position); + l.append(uncertainty); + + return l; +} + +list packetParseGpsCompassEstimatedBaseline(Packet& packet) +{ + list l; + + uint8_t estBaselineUsed; + uint16_t numMeas; + vec3f position; + vec3f uncertainty; + + packet.parseGpsCompassEstimatedBaseline( + &estBaselineUsed, + &numMeas, + &position, + &uncertainty); + + l.append(estBaselineUsed); + l.append(numMeas); + l.append(position); + l.append(uncertainty); + + return l; +} + +list packetParseImuRateConfiguration(Packet& packet) +{ + list l; + + uint16_t imuRate; + uint16_t navDivisor; + float filterTargetRate; + float filterMinRate; + + packet.parseImuRateConfiguration( + &imuRate, + &navDivisor, + &filterTargetRate, + &filterMinRate); + + l.append(imuRate); + l.append(navDivisor); + l.append(filterTargetRate); + l.append(filterMinRate); + + return l; +} + +list packetParseYawPitchRollTrueBodyAccelerationAndAngularRates(Packet& packet) +{ + list l; + + vec3f yawPitchRoll; + vec3f bodyAccel; + vec3f gyro; + + packet.parseYawPitchRollTrueBodyAccelerationAndAngularRates( + &yawPitchRoll, + &bodyAccel, + &gyro); + + l.append(yawPitchRoll); + l.append(bodyAccel); + l.append(gyro); + + return l; +} + +list packetParseYawPitchRollTrueInertialAccelerationAndAngularRates(Packet& packet) +{ + list l; + + vec3f yawPitchRoll; + vec3f inertialAccel; + vec3f gyro; + + packet.parseYawPitchRollTrueInertialAccelerationAndAngularRates( + &yawPitchRoll, + &inertialAccel, + &gyro); + + l.append(yawPitchRoll); + l.append(inertialAccel); + l.append(gyro); + + return l; +} + +BOOST_PYTHON_MODULE(_uart) +{ + enum_("AsciiAsync") + .value("VNOFF", VNOFF) + .value("VNYPR", VNYPR) + .value("VNQTN", VNQTN) + #ifdef INTERNAL + .value("VNQTM", VNQTM) + .value("VNQTA", VNQTA) + .value("VNQTR", VNQTR) + .value("VNQMA", VNQMA) + .value("VNQAR", VNQAR) + #endif + .value("VNQMR", VNQMR) + #ifdef INTERNAL + .value("VNDCM", VNDCM) + #endif + .value("VNMAG", VNMAG) + .value("VNACC", VNACC) + .value("VNGYR", VNGYR) + .value("VNMAR", VNMAR) + .value("VNYMR", VNYMR) + #ifdef INTERNAL + .value("VNYCM", VNYCM) + #endif + .value("VNYBA", VNYBA) + .value("VNYIA", VNYIA) + #ifdef INTERNAL + .value("VNICM", VNICM) + #endif + .value("VNIMU", VNIMU) + .value("VNGPS", VNGPS) + .value("VNGPE", VNGPE) + .value("VNINS", VNINS) + .value("VNINE", VNINE) + .value("VNISL", VNISL) + .value("VNISE", VNISE) + .value("VNDTV", VNDTV) + #ifdef INTERNAL + .value("VNRAW", VNRAW) + .value("VNCMV", VNCMV) + .value("VNSTV", VNSTV) + .value("VNCOV", VNCOV) + #endif + .export_values(); + + enum_("BinaryGroup") + .value("common", BINARYGROUP_COMMON) + .value("time", BINARYGROUP_TIME) + .value("imu", BINARYGROUP_IMU) + .value("gps", BINARYGROUP_GPS) + .value("attitude", BINARYGROUP_ATTITUDE) + .value("ins", BINARYGROUP_INS) + .export_values(); + + enum_("CommonGroup") + .value("none", COMMONGROUP_NONE) + .value("time_startup", COMMONGROUP_TIMESTARTUP) + .value("time_gps", COMMONGROUP_TIMEGPS) + .value("time_sync_in", COMMONGROUP_TIMESYNCIN) + .value("yaw_pitch_roll", COMMONGROUP_YAWPITCHROLL) + .value("quaternion", COMMONGROUP_QUATERNION) + .value("angular_rate", COMMONGROUP_ANGULARRATE) + .value("position", COMMONGROUP_POSITION) + .value("velocity", COMMONGROUP_VELOCITY) + .value("accel", COMMONGROUP_ACCEL) + .value("imu", COMMONGROUP_IMU) + .value("mag_pres", COMMONGROUP_MAGPRES) + .value("delta_theta", COMMONGROUP_DELTATHETA) + .value("ins_status", COMMONGROUP_INSSTATUS) + .value("sync_in_cnt", COMMONGROUP_SYNCINCNT) + .value("time_gps_pps", COMMONGROUP_TIMEGPSPPS) + .export_values(); + + enum_("TimeGroup") + .value("none", TIMEGROUP_NONE) + .value("time_startup", TIMEGROUP_TIMESTARTUP) + .value("time_gps", TIMEGROUP_TIMEGPS) + .value("gps_tow", TIMEGROUP_GPSTOW) + .value("gps_week", TIMEGROUP_GPSWEEK) + .value("time_sync_in", TIMEGROUP_TIMESYNCIN) + .value("time_gps_pps", TIMEGROUP_TIMEGPSPPS) + .value("time_utc", TIMEGROUP_TIMEUTC) + .value("sync_in_cnt", TIMEGROUP_SYNCINCNT) + .export_values(); + + enum_("ImuGroup") + .value("none", IMUGROUP_NONE) + .value("imu_status", IMUGROUP_IMUSTATUS) + .value("uncomp_mag", IMUGROUP_UNCOMPMAG) + .value("uncomp_accel", IMUGROUP_UNCOMPACCEL) + .value("uncomp_gyro", IMUGROUP_UNCOMPGYRO) + .value("temp", IMUGROUP_TEMP) + .value("pres", IMUGROUP_PRES) + .value("delta_theta", IMUGROUP_DELTATHETA) + .value("delta_vel", IMUGROUP_DELTAVEL) + .value("mag", IMUGROUP_MAG) + .value("accel", IMUGROUP_ACCEL) + .value("angular_rate", IMUGROUP_ANGULARRATE) + .value("sens_sat", IMUGROUP_SENSSAT) + .export_values(); + + enum_("GpsGroup") + .value("none", GPSGROUP_NONE) + .value("utc", GPSGROUP_UTC) + .value("tow", GPSGROUP_TOW) + .value("week", GPSGROUP_WEEK) + .value("num_sats", GPSGROUP_NUMSATS) + .value("fix", GPSGROUP_FIX) + .value("pos_lla", GPSGROUP_POSLLA) + .value("pos_ecef", GPSGROUP_POSECEF) + .value("vel_ned", GPSGROUP_VELNED) + .value("vel_ecef", GPSGROUP_VELECEF) + .value("pos_u", GPSGROUP_POSU) + .value("vel_u", GPSGROUP_VELU) + .value("time_u", GPSGROUP_TIMEU) + .export_values(); + + enum_("AttitudeGroup") + .value("none", ATTITUDEGROUP_NONE) + .value("vpe_status", ATTITUDEGROUP_VPESTATUS) + .value("yaw_pitch_roll", ATTITUDEGROUP_YAWPITCHROLL) + .value("quaternion", ATTITUDEGROUP_QUATERNION) + .value("dcm", ATTITUDEGROUP_DCM) + .value("mag_ned", ATTITUDEGROUP_MAGNED) + .value("accel_ned", ATTITUDEGROUP_ACCELNED) + .value("linear_accel_body", ATTITUDEGROUP_LINEARACCELBODY) + .value("linear_accel_ned", ATTITUDEGROUP_LINEARACCELNED) + .value("ypr_u", ATTITUDEGROUP_YPRU) + .export_values(); + + enum_("InsGroup") + .value("none", INSGROUP_NONE) + .value("ins_status", INSGROUP_INSSTATUS) + .value("pos_lla", INSGROUP_POSLLA) + .value("pos_ecef", INSGROUP_POSECEF) + .value("vel_body", INSGROUP_VELBODY) + .value("vel_ecef", INSGROUP_VELECEF) + .value("mag_ecef", INSGROUP_MAGECEF) + .value("accel_ecef", INSGROUP_ACCELECEF) + .value("linear_accel_ecef", INSGROUP_LINEARACCELECEF) + .value("pos_u", INSGROUP_POSU) + .value("vel_u", INSGROUP_VELU) + .export_values(); + + def("identity", static_cast(identity_)); + def("identity", static_cast(identity_)); + def("identity", static_cast(identity_)); + def("identity", static_cast(identity_)); + def("identity", static_cast(identity_)); + def("identity", static_cast(identity_)); + def("identity", static_cast(identity_)); + def("identity", static_cast(identity_)); + + enum_("SyncInMode") + #ifdef INTERNAL + .value("count2", SYNCINMODE_COUNT2) + .value("adc2", SYNCINMODE_ADC2) + .value("async2", SYNCINMODE_ASYNC2) + #endif + .value("count", SYNCINMODE_COUNT) + .value("imu", SYNCINMODE_IMU) + .value("async", SYNCINMODE_ASYNC) + .export_values(); + + enum_("SyncInEdge") + .value("rising", SYNCINEDGE_RISING) + .value("falling", SYNCINEDGE_FALLING) + .export_values(); + + enum_("SyncOutMode") + .value("none", SYNCOUTMODE_NONE) + .value("item_start", SYNCOUTMODE_ITEMSTART) + .value("imu_ready", SYNCOUTMODE_IMUREADY) + .value("ins", SYNCOUTMODE_INS) + .value("gps_pps", SYNCOUTMODE_GPSPPS) + .export_values(); + + enum_("SyncOutPolarity") + .value("negative", SYNCOUTPOLARITY_NEGATIVE) + .value("positive", SYNCOUTPOLARITY_POSITIVE) + .export_values(); + + enum_("CountMode") + .value("none", COUNTMODE_NONE) + .value("sync_in_count", COUNTMODE_SYNCINCOUNT) + .value("sync_in_time", COUNTMODE_SYNCINTIME) + .value("sync_out_counter", COUNTMODE_SYNCOUTCOUNTER) + .value("gps_pps", COUNTMODE_GPSPPS) + .export_values(); + + enum_("StatusMode") + .value("off", STATUSMODE_OFF) + .value("vpe_status", STATUSMODE_VPESTATUS) + .value("ins_status", STATUSMODE_INSSTATUS) + .export_values(); + + enum_("ChecksumMode") + .value("off", CHECKSUMMODE_OFF) + .value("checksum", CHECKSUMMODE_CHECKSUM) + .value("crc", CHECKSUMMODE_CRC) + .export_values(); + + enum_("ErrorMode") + .value("ignore", ERRORMODE_IGNORE) + .value("send", ERRORMODE_SEND) + .value("send_and_off", ERRORMODE_SENDANDOFF) + .export_values(); + + enum_("FilterMode") + .value("no_filtering", FILTERMODE_NOFILTERING) + .value("only_raw", FILTERMODE_ONLYRAW) + .value("only_compensated", FILTERMODE_ONLYCOMPENSATED) + .value("both", FILTERMODE_BOTH) + .export_values(); + + enum_("IntegrationFrame") + .value("body", INTEGRATIONFRAME_BODY) + .value("ned", INTEGRATIONFRAME_NED) + .export_values(); + + enum_("CompensationMode") + .value("none", COMPENSATIONMODE_NONE) + .value("bias", COMPENSATIONMODE_BIAS) + .export_values(); + + enum_("GpsFix") + .value("no_fix", GPSFIX_NOFIX) + .value("time_only", GPSFIX_TIMEONLY) + .value("2d", GPSFIX_2D) + .value("3d", GPSFIX_3D) + .export_values(); + + enum_("GpsMode") + .value("on_board_gps", GPSMODE_ONBOARDGPS) + .value("external_gps", GPSMODE_EXTERNALGPS) + .value("external_vn200_gps", GPSMODE_EXTERNALVN200GPS) + .export_values(); + + enum_("PpsSource") + .value("gps_pps_rising", PPSSOURCE_GPSPPSRISING) + .value("gps_pps_falling", PPSSOURCE_GPSPPSFALLING) + .value("sync_in_rising", PPSSOURCE_SYNCINRISING) + .value("sync_in_falling", PPSSOURCE_SYNCINFALLING) + .export_values(); + + enum_("VpeEnable") + .value("disable", VPEENABLE_DISABLE) + .value("enable", VPEENABLE_ENABLE) + .export_values(); + + enum_("HeadingMode") + .value("absolute", HEADINGMODE_ABSOLUTE) + .value("relative", HEADINGMODE_RELATIVE) + .value("indoor", HEADINGMODE_INDOOR) + .export_values(); + + enum_("VpeMode") + .value("off", VPEMODE_OFF) + .value("mode1", VPEMODE_MODE1) + .export_values(); + + enum_("Scenario") + .value("ahrs", SCENARIO_AHRS) + .value("ins_with_pressure", SCENARIO_INSWITHPRESSURE) + .value("ins_without_pressure", SCENARIO_INSWITHOUTPRESSURE) + .value("gps_moving_baseline_dynamic", SCENARIO_GPSMOVINGBASELINEDYNAMIC) + .value("gps_moving_baseline_static", SCENARIO_GPSMOVINGBASELINESTATIC) + .export_values(); + + enum_("HsiMode") + .value("off", HSIMODE_OFF) + .value("run", HSIMODE_RUN) + .value("reset", HSIMODE_RESET) + .export_values(); + + enum_("HsiOutput") + .value("no_onboard", HSIOUTPUT_NOONBOARD) + .value("use_onboard", HSIOUTPUT_USEONBOARD) + .export_values(); + + enum_("VelocityCompensationMode") + .value("disabled", VELOCITYCOMPENSATIONMODE_DISABLED) + .value("body_measurement", VELOCITYCOMPENSATIONMODE_BODYMEASUREMENT) + .export_values(); + + enum_("MagneticMode") + .value("2d", MAGNETICMODE_2D) + .value("3d", MAGNETICMODE_3D) + .export_values(); + + enum_("ExternalSensorMode") + .value("internal", EXTERNALSENSORMODE_INTERNAL) + .value("external_200hz", EXTERNALSENSORMODE_EXTERNAL200HZ) + .value("external_on_update", EXTERNALSENSORMODE_EXTERNALONUPDATE) + .export_values(); + + enum_("FoamInit") + .value("no_foam_init", FOAMINIT_NOFOAMINIT) + .value("foam_init_pitch_roll", FOAMINIT_FOAMINITPITCHROLL) + .value("foam_init_heading_pitch_roll", FOAMINIT_FOAMINITHEADINGPITCHROLL) + .value("foam_init_pitch_roll_covariance", FOAMINIT_FOAMINITPITCHROLLCOVARIANCE) + .value("foam_init_heading_pitch_roll_covariance", FOAMINIT_FOAMINITHEADINGPITCHROLLCOVARIANCE) + .export_values(); + + enum_("AsyncMode") + .value("none", ASYNCMODE_NONE) + .value("port1", ASYNCMODE_PORT1) + .value("port2", ASYNCMODE_PORT2) + .value("both", ASYNCMODE_BOTH) + .export_values(); + + { + //scope packetScope = class_("Packet", "Structure representing a UART packet received from the VectorNav sensor.", no_init) + scope packetScope = class_("Packet", "Structure representing a UART packet received from the VectorNav sensor.") + //.def(init()) + //.def("__init__", make_constructor(&packetConstructor)) + .def(init()) + .def("is_valid", &Packet::isValid) + //.def("type", &Packet::type) + .def_readonly("type", &Packet::type) + .def("determine_ascii_async_type", &Packet::determineAsciiAsyncType) + .def("groups", &Packet::groups) + .def("group_field", &Packet::groupField) + .def("extract_uint8", &Packet::extractUint8) + .def("extract_int8", &Packet::extractInt8) + .def("extract_uint16", &Packet::extractUint16) + .def("extract_uint32", &Packet::extractUint32) + .def("extract_uint64", &Packet::extractUint64) + .def("extract_float", &Packet::extractFloat) + .def("extract_vec3f", &Packet::extractVec3f) + .def("extract_vec3d", &Packet::extractVec3d) + .def("extract_vec4f", &Packet::extractVec4f) + .def("extract_mat3f", &Packet::extractMat3f) + .def("parse_VNYPR", &packetParseVNYPR) + .def("is_compatible", &isCompatible) + .def("parse_user_tag", &packetParseUserTag) + .def("parse_model_number", &packetParseModelNumber) + .def("parse_hardware_revision", &packetParseHardwareRevision) + .def("parse_serial_number", &packetParseSerialNumber) + .def("parse_firmware_version", &packetParseFirmwareVersion) + .def("parse_serial_baud_rate", &packetParseSerialBaudRate) + .def("parse_async_data_output_type", &packetParseAsyncDataOutputType) + .def("parse_async_data_output_frequency", &packetParseAsyncDataOutputFrequency) + .def("parse_yaw_pitch_roll", &packetParseYawPitchRoll) + .def("parse_attitude_quaternion", &packetParseAttitudeQuaternion) + .def("parse_quaternion_magnetic_acceleration_and_angular_rates", &packetParseQuaternionMagneticAccelerationAndAngularRates) + .def("parse_magnetic_measurements", &packetParseMagneticMeasurements) + .def("parse_acceleration_measurements", &packetParseAccelerationMeasurements) + .def("parse_angular_rate_measurements", &packetParseAngularRateMeasurements) + .def("parse_magnetic_acceleration_and_angular_rates", &packetParseMagneticAccelerationAndAngularRates) + .def("parse_magnetic_and_gravity_reference_vectors", &packetParseMagneticAndGravityReferenceVectors) + .def("parse_filter_measurements_variance_parameters", &packetParseFilterMeasurementsVarianceParameters) + .def("parse_magnetometer_compensation", &packetParseMagnetometerCompensation) + .def("parse_filter_active_tuning_parameters", &packetParseFilterActiveTuningParameters) + .def("parse_acceleration_compensation", &packetParseAccelerationCompensation) + .def("parse_reference_frame_rotation", &packetParseReferenceFrameRotation) + .def("parse_yaw_pitch_roll_magnetic_acceleration_and_angular_rates", &packetParseYawPitchRollMagneticAccelerationAndAngularRates) + .def("parse_communication_protocol_control", &packetParseCommunicationProtocolControl) + .def("parse_synchronization_control", &packetParseSynchronizationControl) + .def("parse_synchronization_status", &packetParseSynchronizationStatus) + .def("parse_filter_basic_control", &packetParseFilterBasicControl) + .def("parse_vpe_basic_control", &packetParseVpeBasicControl) + .def("parse_vpe_magnetometer_basic_tuning", &packetParseVpeMagnetometerBasicTuning) + .def("parse_vpe_magnetometer_advanced_tuning", &packetParseVpeMagnetometerAdvancedTuning) + .def("parse_vpe_accelerometer_basic_tuning", &packetParseVpeAccelerometerBasicTuning) + .def("parse_vpe_accelerometer_advanced_tuning", &packetParseVpeAccelerometerAdvancedTuning) + .def("parse_vpe_gryo_basic_tuning", &packetParseVpeGryoBasicTuning) + .def("parse_filter_startup_gyro_bias", &packetParseFilterStartupGyroBias) + .def("parse_magnetometer_calibration_control", &packetParseMagnetometerCalibrationControl) + .def("parse_calculated_magnetometer_calibration", &packetParseCalculatedMagnetometerCalibration) + .def("parse_indoor_heading_mode_control", &packetParseIndoorHeadingModeControl) + .def("parse_velocity_compensation_measurement", &packetParseVelocityCompensationMeasurement) + .def("parse_velocity_compensation_control", &packetParseVelocityCompensationControl) + .def("parse_velocity_compensation_status", &packetParseVelocityCompensationStatus) + .def("parse_imu_measurements", &packetParseImuMeasurements) + .def("parse_gps_configuration", &packetParseGpsConfiguration) + .def("parse_gps_antenna_offset", &packetParseGpsAntennaOffset) + .def("parse_gps_solution-lla", &packetParseGpsSolutionLla) + .def("parse_gps_solution-ecef", &packetParseGpsSolutionEcef) + .def("parse_ins_solution-lla", &packetParseInsSolutionLla) + .def("parse_ins_solution-ecef", &packetParseInsSolutionEcef) + .def("parse_ins_advanced_configuration", &packetParseInsAdvancedConfiguration) + .def("parse_ins_state-lla", &packetParseInsStateLla) + .def("parse_ins_state-ecef", &packetParseInsStateEcef) + .def("parse_startup_filter_bias_estimate", &packetParseStartupFilterBiasEstimate) + .def("parse_delta_theta_and_delta_velocity", &packetParseDeltaThetaAndDeltaVelocity) + .def("parse_delta_theta_and_delta_velocity_configuration", &packetParseDeltaThetaAndDeltaVelocityConfiguration) + .def("parse_reference_vector_configuration", &packetParseReferenceVectorConfiguration) + .def("parse_gyro_compensation", &packetParseGyroCompensation) + .def("parse_imu_filtering_configuration", &packetParseImuFilteringConfiguration) + .def("parse_gps_compass_baseline", &packetParseGpsCompassBaseline) + .def("parse_gps_compass_estimated_baseline", &packetParseGpsCompassEstimatedBaseline) + .def("parse_imu_rate_configuration", &packetParseImuRateConfiguration) + .def("parse_yaw_pitch_roll_true_body_acceleration_and_angular_rates", &packetParseYawPitchRollTrueBodyAccelerationAndAngularRates) + .def("parse_yaw_pitch_roll_true_inertial_acceleration_and_angular_rates", &packetParseYawPitchRollTrueInertialAccelerationAndAngularRates) + ; + + //.def("determineAsciiAsyncType", &Packet::determineAsciiAsyncType, "Determines the type of ASCII asynchronous message this packet is.") + //.def("parseVNYPR", &Packet::parseVNYPR, args("The yaw, pitch, roll values in the packet."), "Parses a VNYPR asynchronous packet.") + + enum_("Type") + .value("unknown", Packet::TYPE_UNKNOWN) + .value("binary", Packet::TYPE_BINARY) + .value("ascii", Packet::TYPE_ASCII) + .export_values(); + } + + class_("PacketFinder") + .def("process_received_data", static_cast(&PacketFinder::processReceivedData)) + //.def("register_packet_found_handler", &PacketFinder::register_packet_found_handler) + //.def("test_callback", &test_callback2)py + .def("register_packet_found_handler", &PacketFinder::register_packet_found_handler, return_internal_reference<>()) + ; + +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/protocol/uart/types.cpp b/ext/vnproglib-1.1.0.115/src/vn/protocol/uart/types.cpp new file mode 100755 index 0000000..753735f --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/protocol/uart/types.cpp @@ -0,0 +1,59 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/protocol/uart/types.h" + +namespace vn { +namespace protocol { +namespace uart { + +VpeStatus::VpeStatus() +{ +} + +VpeStatus::VpeStatus(uint16_t raw) +{ + attitudeQuality = 0x0003 & raw; + gyroSaturation = (0x0004 & raw) != 0; + gyroSaturationRecovery = (0x0008 & raw) != 0; + magDisturbance = (0x0030 & raw) >> 4; + magSaturation = (0x0040 & raw) != 0; + accDisturbance = (0x0180 & raw) >> 7; + accSaturation = (0x0200 & raw) != 0; + knownMagDisturbance = (0x0800 & raw) != 0; + knownAccelDisturbance = (0x1000 & raw) != 0; +} + + +CommonGroup operator|(CommonGroup lhs, CommonGroup rhs) +{ + return CommonGroup(int(lhs) | int(rhs)); +} + +TimeGroup operator|(TimeGroup lhs, TimeGroup rhs) +{ + return TimeGroup(int(lhs) | int(rhs)); +} + +ImuGroup operator|(ImuGroup lhs, ImuGroup rhs) +{ + return ImuGroup(int(lhs) | int(rhs)); +} + +GpsGroup operator|(GpsGroup lhs, GpsGroup rhs) +{ + return GpsGroup(int(lhs) | int(rhs)); +} + +AttitudeGroup operator|(AttitudeGroup lhs, AttitudeGroup rhs) +{ + return AttitudeGroup(int(lhs) | int(rhs)); +} + +InsGroup operator|(InsGroup lhs, InsGroup rhs) +{ + return InsGroup(int(lhs) | int(rhs)); +} + +} +} +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/protocol/uart/util.cpp b/ext/vnproglib-1.1.0.115/src/vn/protocol/uart/util.cpp new file mode 100755 index 0000000..fc8cf75 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/protocol/uart/util.cpp @@ -0,0 +1,661 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/protocol/uart/util.h" + +#include "vn/exceptions.h" + +using namespace std; + +namespace vn { +namespace protocol { +namespace uart { + +char* vnstrtok(char* str, size_t& startIndex); + +string str(AsciiAsync val) +{ + switch (val) + { + case VNOFF: + return "OFF"; + case VNYPR: + return "VNYPR"; + case VNQTN: + return "VNQTN"; + #ifdef INTERNAL + case VNQTM: + return "VNQTM"; + case VNQTA: + return "VNQTA"; + case VNQTR: + return "VNQTR"; + case VNQMA: + return "VNQMA"; + case VNQAR: + return "VNQAR"; + #endif + case VNQMR: + return "VNQMR"; + #ifdef INTERNAL + case VNDCM: + return "VNDCM"; + #endif + case VNMAG: + return "VNMAG"; + case VNACC: + return "VNACC"; + case VNGYR: + return "VNGYR"; + case VNMAR: + return "VNMAR"; + case VNYMR: + return "VNYMR"; + #ifdef INTERNAL + case VNYCM: + return "VNYCM"; + #endif + case VNYBA: + return "VNYBA"; + case VNYIA: + return "VNYIA"; + #ifdef INTERNAL + case VNICM: + return "VNICM"; + #endif + case VNIMU: + return "VNIMU"; + case VNGPS: + return "VNGPS"; + case VNGPE: + return "VNGPE"; + case VNINS: + return "VNINS"; + case VNINE: + return "VNINE"; + case VNISL: + return "VNISL"; + case VNISE: + return "VNISE"; + case VNDTV: + return "VNDTV"; + #ifdef INTERNAL + case VNRAW: + return "VNRAW"; + case VNCMV: + return "VNCMV"; + case VNSTV: + return "VNSTV"; + case VNCOV: + return "VNCOV"; + #endif + default: + throw invalid_argument("val"); + } +} + +ostream& operator<<(ostream& out, AsciiAsync e) +{ + out << str(e); + return out; +} + +string str(SensorError val) +{ + switch (val) + { + case ERR_HARD_FAULT: + return "HardFault"; + case ERR_SERIAL_BUFFER_OVERFLOW: + return "SerialBufferOverflow"; + case ERR_INVALID_CHECKSUM: + return "InvalidChecksum"; + case ERR_INVALID_COMMAND: + return "InvalidCommand"; + case ERR_NOT_ENOUGH_PARAMETERS: + return "NotEnoughParameters"; + case ERR_TOO_MANY_PARAMETERS: + return "TooManyParameters"; + case ERR_INVALID_PARAMETER: + return "InvalidParameter"; + case ERR_INVALID_REGISTER: + return "InvalidRegister"; + case ERR_UNAUTHORIZED_ACCESS: + return "UnauthorizedAccess"; + case ERR_WATCHDOG_RESET: + return "WatchdogReset"; + case ERR_OUTPUT_BUFFER_OVERFLOW: + return "OutputBufferOverflow"; + case ERR_INSUFFICIENT_BAUD_RATE: + return "InsufficientBaudRate"; + case ERR_ERROR_BUFFER_OVERFLOW: + return "ErrorBufferOverflow"; + default: + throw invalid_argument("val"); + } +} + +ostream& operator<<(ostream& out, SensorError e) +{ + out << str(e); + return out; +} + +string str(SyncInMode val) +{ + switch (val) + { + #ifdef INTERNAL + case SYNCINMODE_COUNT2: + return "Count2"; + case SYNCINMODE_ADC2: + return "Adc2"; + case SYNCINMODE_ASYNC2: + return "Async2"; + #endif + case SYNCINMODE_COUNT: + return "Count"; + case SYNCINMODE_IMU: + return "Imu"; + case SYNCINMODE_ASYNC: + return "Async"; + default: + throw invalid_argument("val"); + } +} + +string str(SyncInEdge val) +{ + switch (val) + { + case SYNCINEDGE_RISING: + return "Rising"; + case SYNCINEDGE_FALLING: + return "Falling"; + default: + throw invalid_argument("val"); + } +} + +string str(SyncOutMode val) +{ + switch (val) + { + case SYNCOUTMODE_NONE: + return "None"; + case SYNCOUTMODE_ITEMSTART: + return "ItemStart"; + case SYNCOUTMODE_IMUREADY: + return "ImuReady"; + case SYNCOUTMODE_INS: + return "Ins"; + case SYNCOUTMODE_GPSPPS: + return "GpsPps"; + default: + throw invalid_argument("val"); + } +} + +string str(SyncOutPolarity val) +{ + switch (val) + { + case SYNCOUTPOLARITY_NEGATIVE: + return "Negative"; + case SYNCOUTPOLARITY_POSITIVE: + return "Positive"; + default: + throw invalid_argument("val"); + } +} + +string str(CountMode val) +{ + switch (val) + { + case COUNTMODE_NONE: + return "None"; + case COUNTMODE_SYNCINCOUNT: + return "SyncInCount"; + case COUNTMODE_SYNCINTIME: + return "SyncInTime"; + case COUNTMODE_SYNCOUTCOUNTER: + return "SyncOutCounter"; + case COUNTMODE_GPSPPS: + return "GpsPps"; + default: + throw invalid_argument("val"); + } +} + +string str(StatusMode val) +{ + switch (val) + { + case STATUSMODE_OFF: + return "Off"; + case STATUSMODE_VPESTATUS: + return "VpeStatus"; + case STATUSMODE_INSSTATUS: + return "InsStatus"; + default: + throw invalid_argument("val"); + } +} + +string str(ChecksumMode val) +{ + switch (val) + { + case CHECKSUMMODE_OFF: + return "Off"; + case CHECKSUMMODE_CHECKSUM: + return "Checksum"; + case CHECKSUMMODE_CRC: + return "Crc"; + default: + throw invalid_argument("val"); + } +} + +string str(ErrorMode val) +{ + switch (val) + { + case ERRORMODE_IGNORE: + return "Ignore"; + case ERRORMODE_SEND: + return "Send"; + case ERRORMODE_SENDANDOFF: + return "SendAndOff"; + default: + throw invalid_argument("val"); + } +} + +string str(FilterMode val) +{ + switch (val) + { + case FILTERMODE_NOFILTERING: + return "NoFiltering"; + case FILTERMODE_ONLYRAW: + return "OnlyRaw"; + case FILTERMODE_ONLYCOMPENSATED: + return "OnlyCompensated"; + case FILTERMODE_BOTH: + return "Both"; + default: + throw invalid_argument("val"); + } +} + +string str(IntegrationFrame val) +{ + switch (val) + { + case INTEGRATIONFRAME_BODY: + return "Body"; + case INTEGRATIONFRAME_NED: + return "Ned"; + default: + throw invalid_argument("val"); + } +} + +string str(CompensationMode val) +{ + switch (val) + { + case COMPENSATIONMODE_NONE: + return "None"; + case COMPENSATIONMODE_BIAS: + return "Bias"; + default: + throw invalid_argument("val"); + } +} + +string str(GpsFix val) +{ + switch (val) + { + case GPSFIX_NOFIX: + return "NoFix"; + case GPSFIX_TIMEONLY: + return "TimeOnly"; + case GPSFIX_2D: + return "2D"; + case GPSFIX_3D: + return "3D"; + default: + throw invalid_argument("val"); + } +} + +string str(GpsMode val) +{ + switch (val) + { + case GPSMODE_ONBOARDGPS: + return "OnBoardGps"; + case GPSMODE_EXTERNALGPS: + return "ExternalGps"; + case GPSMODE_EXTERNALVN200GPS: + return "ExternalVn200Gps"; + default: + throw invalid_argument("val"); + } +} + +string str(PpsSource val) +{ + switch (val) + { + case PPSSOURCE_GPSPPSRISING: + return "GpsPpsRising"; + case PPSSOURCE_GPSPPSFALLING: + return "GpsPpsFalling"; + case PPSSOURCE_SYNCINRISING: + return "SyncInRising"; + case PPSSOURCE_SYNCINFALLING: + return "SyncInFalling"; + default: + throw invalid_argument("val"); + } +} + +string str(VpeEnable val) +{ + switch (val) + { + case VPEENABLE_DISABLE: + return "Disable"; + case VPEENABLE_ENABLE: + return "Enable"; + default: + throw invalid_argument("val"); + } +} + +string str(HeadingMode val) +{ + switch (val) + { + case HEADINGMODE_ABSOLUTE: + return "Absolute"; + case HEADINGMODE_RELATIVE: + return "Relative"; + case HEADINGMODE_INDOOR: + return "Indoor"; + default: + throw invalid_argument("val"); + } +} + +string str(VpeMode val) +{ + switch (val) + { + case VPEMODE_OFF: + return "Off"; + case VPEMODE_MODE1: + return "Mode1"; + default: + throw invalid_argument("val"); + } +} + +string str(Scenario val) +{ + switch (val) + { + case SCENARIO_AHRS: + return "Ahrs"; + case SCENARIO_INSWITHPRESSURE: + return "InsWithPressure"; + case SCENARIO_INSWITHOUTPRESSURE: + return "InsWithoutPressure"; + case SCENARIO_GPSMOVINGBASELINEDYNAMIC: + return "GpsMovingBaselineDynamic"; + case SCENARIO_GPSMOVINGBASELINESTATIC: + return "GpsMovingBaselineStatic"; + default: + throw invalid_argument("val"); + } +} + +string str(HsiMode val) +{ + switch (val) + { + case HSIMODE_OFF: + return "Off"; + case HSIMODE_RUN: + return "Run"; + case HSIMODE_RESET: + return "Reset"; + default: + throw invalid_argument("val"); + } +} + +string str(HsiOutput val) +{ + switch (val) + { + case HSIOUTPUT_NOONBOARD: + return "NoOnboard"; + case HSIOUTPUT_USEONBOARD: + return "UseOnboard"; + default: + throw invalid_argument("val"); + } +} + +string str(VelocityCompensationMode val) +{ + switch (val) + { + case VELOCITYCOMPENSATIONMODE_DISABLED: + return "Disabled"; + case VELOCITYCOMPENSATIONMODE_BODYMEASUREMENT: + return "BodyMeasurement"; + default: + throw invalid_argument("val"); + } +} + +string str(MagneticMode val) +{ + switch (val) + { + case MAGNETICMODE_2D: + return "2D"; + case MAGNETICMODE_3D: + return "3D"; + default: + throw invalid_argument("val"); + } +} + +string str(ExternalSensorMode val) +{ + switch (val) + { + case EXTERNALSENSORMODE_INTERNAL: + return "Internal"; + case EXTERNALSENSORMODE_EXTERNAL200HZ: + return "External200Hz"; + case EXTERNALSENSORMODE_EXTERNALONUPDATE: + return "ExternalOnUpdate"; + default: + throw invalid_argument("val"); + } +} + +string str(FoamInit val) +{ + switch (val) + { + case FOAMINIT_NOFOAMINIT: + return "NoFoamInit"; + case FOAMINIT_FOAMINITPITCHROLL: + return "FoamInitPitchRoll"; + case FOAMINIT_FOAMINITHEADINGPITCHROLL: + return "FoamInitHeadingPitchRoll"; + case FOAMINIT_FOAMINITPITCHROLLCOVARIANCE: + return "FoamInitPitchRollCovariance"; + case FOAMINIT_FOAMINITHEADINGPITCHROLLCOVARIANCE: + return "FoamInitHeadingPitchRollCovariance"; + default: + throw invalid_argument("val"); + } +} + +ostream& operator<<(ostream& out, SyncInMode e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, SyncInEdge e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, SyncOutMode e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, SyncOutPolarity e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, CountMode e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, StatusMode e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, ChecksumMode e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, ErrorMode e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, FilterMode e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, IntegrationFrame e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, CompensationMode e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, GpsFix e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, GpsMode e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, PpsSource e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, VpeEnable e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, HeadingMode e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, VpeMode e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, Scenario e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, HsiMode e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, HsiOutput e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, VelocityCompensationMode e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, MagneticMode e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, ExternalSensorMode e) +{ + out << str(e); + return out; +} + +ostream& operator<<(ostream& out, FoamInit e) +{ + out << str(e); + return out; +} + +} +} +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/python.cpp b/ext/vnproglib-1.1.0.115/src/vn/python.cpp new file mode 100755 index 0000000..f36cf54 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/python.cpp @@ -0,0 +1,240 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/util/boostpython.h" +#include "boost/python/raw_function.hpp" +//#include "boost/python/errors.hpp" +//#include "boost/python/base_type_traits.hpp" + +#if _WIN32 + #include +#endif + +#include "vn/exceptions.h" +#include "vn/event.h" +#include "vn/utilities.h" + +// TEMP +#include +using namespace std; + +namespace bp = boost::python; + +#if _WIN32 + +void setDllLoadPaths(HINSTANCE histDll); +void unsetDllLoadPaths(); + +#if REVIEW +LONG WINAPI unhandledExceptionHandler(_In_ struct _EXCEPTION_POINTERS *ExceptionInfo) +{ + cout << "EXCEPTION################################" << endl; + + //DWORD temp = GetExceptionCode(); + + if (ExceptionInfo == NULL) + cout << "ITS NULL!" << flush << endl; + else + cout << "NOT NULL" << flush << endl; + + try + { + cout << "INFO: " << (long)ExceptionInfo << flush << endl; + + cout << "CODE: " << (int)(ExceptionInfo->ExceptionRecord->ExceptionCode) << flush << endl; + + cout << "Made Past" << flush << endl; + } + catch (...) + { + cout << "EXCEPTION HERE!" << flush << endl; + } + + return 0; +} +#endif + +BOOL WINAPI DllMain(HINSTANCE histDll, DWORD fdwReason, LPVOID) +{ + if (fdwReason == DLL_PROCESS_ATTACH) + { + setDllLoadPaths(histDll); + } + else if (fdwReason == DLL_PROCESS_DETACH) + { + unsetDllLoadPaths(); + } + + //SetUnhandledExceptionFilter(unhandledExceptionHandler); + + return true; +} + +// Sets the DLL paths so that the other *.pyd files can load their dependancies. +void setDllLoadPaths(HINSTANCE histDll) +{ + // Get the folder containing _vnpy.pyd. + char vnpyPydPath[MAX_PATH] = { 0 }; + GetModuleFileName(histDll, vnpyPydPath, _countof(vnpyPydPath)); + string path(vnpyPydPath); + string dlldir = path.substr(0, path.find_last_of("\\/")); + + SetDllDirectory(dlldir.c_str()); +} + +void unsetDllLoadPaths() +{ + SetDllDirectory(NULL); +} + +#endif + +void timeoutTranslator(vn::timeout const& e) +{ + PyErr_SetString(PyExc_RuntimeError, e.what()); +} + +void invalidOperationTranslator(vn::invalid_operation const& e) +{ + PyErr_SetString(PyExc_RuntimeError, e.what()); +} + +void dimensionErrorTranslator(vn::dimension_error const& e) +{ + PyErr_SetString(PyExc_RuntimeError, e.what()); +} + +void unknownErrorTranslator(vn::unknown_error const& e) +{ + PyErr_SetString(PyExc_RuntimeError, e.what()); +} + +void notImplementedTranslator(vn::not_implemented const& e) +{ + PyErr_SetString(PyExc_NotImplementedError, e.what()); +} + +void nullPointerTranslator(vn::null_pointer const& e) +{ + PyErr_SetString(PyExc_RuntimeError, e.what()); +} + +void permissionDeniedTranslator(vn::permission_denied const& e) +{ + PyErr_SetString(PyExc_RuntimeError, e.what()); +} + +void notSupportedTranslator(vn::not_supported const& e) +{ + PyErr_SetString(PyExc_RuntimeError, e.what()); +} + +void notFoundTranslator(vn::not_found const& e) +{ + PyErr_SetString(PyExc_FileNotFoundError, e.what()); +} + +void invalidFormatTranslator(vn::invalid_format const& e) +{ + PyErr_SetString(PyExc_RuntimeError, e.what()); +} + +bp::tuple test_fire(bp::tuple args, bp::dict kwargs) +{ + cout << "It's working" << flush << endl; + return bp::make_tuple(); +} + +//class __declspec(dllexport) TestRaw +class TestRaw +{ + public: + bp::object testing(bp::tuple args, bp::dict kwargs) + { + cout << "Made 1" << flush << endl; + //return bp::make_tuple(); + return bp::object(); + } +}; + +bp::object test_this(TestRaw* raw, bp::tuple args, bp::dict kwargs) +{ + return raw->testing(args, kwargs); +} + +//bp::object testing_global() + +std::string checkDllValidity(std::string dll, std::string workingDir) +{ + std::string returnString; + std::vector missingDlls; + + if(!vn::checkDllValidity(dll, workingDir, missingDlls)) + { + for (size_t index = 0; index < missingDlls.size(); ++index) + { + returnString += missingDlls[index]; + if (index < missingDlls.size() - 1) + { + returnString += "\n"; + } + } + } + + return returnString; +} + +BOOST_PYTHON_MODULE(_vnpy) +{ + //PyEval_InitThreads(); + + //docstring_options local_docstring_options(true, true, false); + + bp::def("check_dll_validity", &::checkDllValidity); + + bp::register_exception_translator(&timeoutTranslator); + bp::register_exception_translator(&invalidOperationTranslator); + bp::register_exception_translator(&dimensionErrorTranslator); + bp::register_exception_translator(&unknownErrorTranslator); + bp::register_exception_translator(¬ImplementedTranslator); + bp::register_exception_translator(&nullPointerTranslator); + bp::register_exception_translator(&permissionDeniedTranslator); + bp::register_exception_translator(¬SupportedTranslator); + bp::register_exception_translator(¬FoundTranslator); + bp::register_exception_translator(&invalidFormatTranslator); + + +// bp::class_("Event", "Allows registration for notification when an event occurs.") + //.def("fire", static_cast(&vn::Event::fire)) + //.def("fire", bp::raw_function(vn::Event::fire)) +// .def("fire", bp::raw_function(&vn::Event::fire)) + //.def("fire", raw_function(test_fire)) + //.def("fire", raw_function(static_cast(&vn::Event::fire), 1)) +// .def("add", &vn::Event::add) +// ; + +// bp::class_("TestRaw") +// .def("testing", bp::raw_function(&TestRaw::testing)) + //.def("testing", bp::raw_function(&TestRaw::testing, 1)) + //.def("testing", bp::raw_function(&test_this, 1)) +// ; + + //def __init__(self): + // self.__handlers = [] + + //def __iadd__(self, handler): + // self.__handlers.append(handler) + // return self + + //def __isub__(self, handler): + // self.__handlers.remove(handler) + // return self + + //def fire(self, *args, **keywargs): + // for handler in self.__handlers: + // handler(*args, **keywargs) + + //def clearObjectHandlers(self, inObject): + // for theHandler in self.__handlers: + // if theHandler.im_self == inObject: + // self -= theHandler +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/sensors/compositedata.cpp b/ext/vnproglib-1.1.0.115/src/vn/sensors/compositedata.cpp new file mode 100755 index 0000000..769de57 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/sensors/compositedata.cpp @@ -0,0 +1,2161 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/sensors/compositedata.h" +#include "vn/math/conversions.h" + +using namespace std; +using namespace vn::math; +using namespace vn::protocol::uart; + +namespace vn { +namespace sensors { + +typedef vector::iterator cditer; + +struct CompositeData::Impl +{ + enum AttitudeType + { + CDATT_None, + CDATT_YawPitchRoll, + CDATT_Quaternion, + CDATT_DirectionCosineMatrix + }; + + enum AccelerationType + { + CDACC_None, + CDACC_Normal, + CDACC_Uncompensated, + CDACC_LinearBody, + CDACC_LinearNed, + CDACC_Ned, + CDACC_Ecef, + CDACC_LinearEcef, + }; + + enum MagneticType + { + CDMAG_None, + CDMAG_Normal, + CDMAG_Uncompensated, + CDMAG_Ned, + CDMAG_Ecef, + }; + + enum AngularRateType + { + CDANR_None, + CDANR_Normal, + CDANR_Uncompensated, + }; + + enum TemperatureType + { + CDTEM_None, + CDTEM_Normal, + }; + + enum PressureType + { + CDPRE_None, + CDPRE_Normal + }; + + enum PositionType + { + CDPOS_None, + CDPOS_GpsLla, + CDPOS_GpsEcef, + CDPOS_EstimatedLla, + CDPOS_EstimatedEcef + }; + + enum VelocityType + { + CDVEL_None, + CDVEL_GpsNed, + CDVEL_GpsEcef, + CDVEL_EstimatedNed, + CDVEL_EstimatedEcef, + CDVEL_EstimatedBody + }; + + enum PositionUncertaintyType + { + CDPOU_None, + CDPOU_GpsNed, + CDPOU_GpsEcef, + CDPOU_Estimated + }; + + enum VelocityUncertaintyType + { + CDVEU_None, + CDVEU_Gps, + CDVEU_Estimated + }; + + AttitudeType mostRecentlyUpdatedAttitudeType; + MagneticType mostRecentlyUpdatedMagneticType; + AccelerationType mostRecentlyUpdatedAccelerationType; + AngularRateType mostRecentlyUpdatedAngularRateType; + TemperatureType mostRecentlyUpdatedTemperatureType; + PressureType mostRecentlyUpdatePressureType; + PositionType mostRecentlyUpdatedPositionType; + VelocityType mostRecentlyUpdatedVelocityType; + PositionUncertaintyType mostRecentlyUpdatedPositionUncertaintyType; + VelocityUncertaintyType mostRecentlyUpdatedVelocityUncertaintyType; + bool hasYawPitchRoll, hasQuaternion, hasDirectionCosineMatrix, + hasMagnetic, hasMagneticUncompensated, hasMagneticNed, hasMagneticEcef, + hasAcceleration, hasAccelerationLinearBody, hasAccelerationUncompensated, hasAccelerationLinearNed, hasAccelerationLinearEcef, hasAccelerationNed, hasAccelerationEcef, + hasAngularRate, hasAngularRateUncompensated, + hasTemperature, hasPressure, + hasPositionGpsLla, hasPositionGpsEcef, hasPositionEstimatedLla, hasPositionEstimatedEcef, + hasVelocityGpsNed, hasVelocityGpsEcef, hasVelocityEstimatedNed, hasVelocityEstimatedEcef, hasVelocityEstimatedBody, + hasDeltaTime, hasDeltaTheta, hasDeltaVelocity, + hasTimeStartup, hasTimeGps, hasTow, hasWeek, hasNumSats, hasTimeSyncIn, hasVpeStatus, hasInsStatus, + hasSyncInCnt, hasTimeGpsPps, hasGpsTow, hasTimeUtc, hasSensSat, hasFix, + hasPositionUncertaintyGpsNed, hasPositionUncertaintyGpsEcef, hasPositionUncertaintyEstimated, + hasVelocityUncertaintyGps, hasVelocityUncertaintyEstimated, hasTimeUncertainty, hasAttitudeUncertainty; + vec3f yawPitchRoll, + magnetic, magneticUncompensated, magneticNed, magneticEcef, + acceleration, accelerationLinearBody, accelerationUncompensated, accelerationLinearNed, accelerationLinearEcef, accelerationNed, accelerationEcef, + angularRate, angularRateUncompensated, + velocityGpsNed, velocityGpsEcef, velocityEstimatedNed, velocityEstimatedEcef, velocityEstimatedBody, + deltaTheta, deltaVelocity, positionUncertaintyGpsNed, positionUncertaintyGpsEcef, attitudeUncertainty; + vec3d positionGpsLla, positionGpsEcef, positionEstimatedLla, positionEstimatedEcef; + vec4f quaternion; + mat3f directionConsineMatrix; + float temperature, pressure, deltaTime, positionUncertaintyEstimated, + velocityUncertaintyGps, velocityUncertaintyEstimated; + uint64_t timeStartup, timeGps, timeSyncIn, timeGpsPps, gpsTow; + double tow; + uint16_t week; + uint8_t numSats; + VpeStatus vpeStatus; + InsStatus insStatus; + uint32_t syncInCnt, timeUncertainty; + GpsFix fix; + TimeUtc timeUtc; + SensSat sensSat; + + void reset() + { + mostRecentlyUpdatedAttitudeType = CDATT_None; + mostRecentlyUpdatedMagneticType = CDMAG_None; + mostRecentlyUpdatedAccelerationType = CDACC_None; + mostRecentlyUpdatedAngularRateType = CDANR_None; + mostRecentlyUpdatedTemperatureType = CDTEM_None; + mostRecentlyUpdatePressureType = CDPRE_None; + mostRecentlyUpdatedPositionType = CDPOS_None; + mostRecentlyUpdatedVelocityType = CDVEL_None; + mostRecentlyUpdatedPositionUncertaintyType = CDPOU_None; + mostRecentlyUpdatedVelocityUncertaintyType = CDVEU_None; + hasYawPitchRoll = false; + hasQuaternion = false; + hasDirectionCosineMatrix = false; + hasMagnetic = false; + hasMagneticUncompensated = false; + hasMagneticNed = false; + hasMagneticEcef = false; + hasAcceleration = false; + hasAccelerationLinearBody = false; + hasAccelerationUncompensated = false; + hasAccelerationLinearNed = false; + hasAccelerationLinearEcef = false; + hasAccelerationNed = false; + hasAccelerationEcef = false; + hasAngularRate = false; + hasAngularRateUncompensated = false; + hasTemperature = false; + hasPressure = false; + hasPositionGpsLla = false; + hasPositionGpsEcef = false; + hasPositionEstimatedLla = false; + hasPositionEstimatedEcef = false; + hasVelocityGpsNed = false; + hasVelocityGpsEcef = false; + hasVelocityEstimatedNed = false; + hasVelocityEstimatedEcef = false; + hasVelocityEstimatedBody = false; + hasDeltaTime = false; + hasDeltaTheta = false; + hasDeltaVelocity = false; + hasTimeStartup = false; + hasTimeGps = false; + hasTow = false; + hasWeek = false; + hasNumSats = false; + hasTimeSyncIn = false; + hasVpeStatus = false; + hasInsStatus = false; + hasSyncInCnt = false; + hasTimeGpsPps = false; + hasGpsTow = false; + hasTimeUtc = false; + hasSensSat = false; + hasFix = false; + hasPositionUncertaintyGpsNed = false; + hasPositionUncertaintyGpsEcef = false; + hasPositionUncertaintyEstimated = false; + hasVelocityUncertaintyGps = false; + hasVelocityUncertaintyEstimated = false; + hasTimeUncertainty = false; + hasAttitudeUncertainty = false; + } + + void setYawPitchRoll(vec3f ypr) + { + mostRecentlyUpdatedAttitudeType = CDATT_YawPitchRoll; + hasYawPitchRoll = true; + yawPitchRoll = ypr; + } + + void setQuaternion(vec4f quat) + { + mostRecentlyUpdatedAttitudeType = CDATT_Quaternion; + hasQuaternion = true; + quaternion = quat; + } + + void setDirectionConsineMatrix(mat3f dcm) + { + mostRecentlyUpdatedAttitudeType = CDATT_DirectionCosineMatrix; + hasDirectionCosineMatrix = true; + directionConsineMatrix = dcm; + } + + void setMagnetic(vec3f mag) + { + mostRecentlyUpdatedMagneticType = CDMAG_Normal; + hasMagnetic = true; + magnetic = mag; + } + + void setMagneticUncompensated(vec3f mag) + { + mostRecentlyUpdatedMagneticType = CDMAG_Uncompensated; + hasMagneticUncompensated = true; + magneticUncompensated = mag; + } + + void setMagneticNed(vec3f mag) + { + mostRecentlyUpdatedMagneticType = CDMAG_Ned; + hasMagneticNed = true; + magneticNed = mag; + } + + void setMagneticEcef(vec3f mag) + { + mostRecentlyUpdatedMagneticType = CDMAG_Ecef; + hasMagneticEcef = true; + magneticEcef = mag; + } + + + void setAcceleration(vec3f accel) + { + mostRecentlyUpdatedAccelerationType = CDACC_Normal; + hasAcceleration = true; + acceleration = accel; + } + + void setAccelerationLinearBody(vec3f accel) + { + mostRecentlyUpdatedAccelerationType = CDACC_LinearBody; + hasAccelerationLinearBody = true; + accelerationLinearBody = accel; + } + + void setAccelerationUncompensated(vec3f accel) + { + mostRecentlyUpdatedAccelerationType = CDACC_Uncompensated; + hasAccelerationUncompensated = true; + accelerationUncompensated = accel; + } + + void setAccelerationLinearNed(vec3f accel) + { + mostRecentlyUpdatedAccelerationType = CDACC_LinearNed; + hasAccelerationLinearNed = true; + accelerationLinearNed = accel; + } + + void setAccelerationLinearEcef(vec3f accel) + { + mostRecentlyUpdatedAccelerationType = CDACC_LinearEcef; + hasAccelerationLinearEcef = true; + accelerationLinearEcef = accel; + } + + void setAccelerationNed(vec3f accel) + { + mostRecentlyUpdatedAccelerationType = CDACC_Ned; + hasAccelerationNed = true; + accelerationNed = accel; + } + + void setAccelerationEcef(vec3f accel) + { + mostRecentlyUpdatedAccelerationType = CDACC_Ecef; + hasAccelerationEcef = true; + accelerationEcef = accel; + } + + + void setAngularRate(vec3f ar) + { + mostRecentlyUpdatedAngularRateType = CDANR_Normal; + hasAngularRate = true; + angularRate = ar; + } + + void setAngularRateUncompensated(vec3f ar) + { + mostRecentlyUpdatedAngularRateType = CDANR_Uncompensated; + hasAngularRateUncompensated = true; + angularRateUncompensated = ar; + } + + + void setTemperature(float temp) + { + mostRecentlyUpdatedTemperatureType = CDTEM_Normal; + hasTemperature = true; + temperature = temp; + } + + + void setPressure(float pres) + { + mostRecentlyUpdatePressureType = CDPRE_Normal; + hasPressure = true; + pressure = pres; + } + + void setPositionGpsLla(vec3d pos) + { + mostRecentlyUpdatedPositionType = CDPOS_GpsLla; + hasPositionGpsLla = true; + positionGpsLla = pos; + } + + void setPositionGpsEcef(vec3d pos) + { + mostRecentlyUpdatedPositionType = CDPOS_GpsEcef; + hasPositionGpsEcef = true; + positionGpsEcef = pos; + } + + void setPositionEstimatedLla(vec3d pos) + { + mostRecentlyUpdatedPositionType = CDPOS_EstimatedLla; + hasPositionEstimatedLla = true; + positionEstimatedLla = pos; + } + + void setPositionEstimatedEcef(vec3d pos) + { + mostRecentlyUpdatedPositionType = CDPOS_EstimatedEcef; + hasPositionEstimatedEcef = true; + positionEstimatedEcef = pos; + } + + void setVelocityGpsNed(vec3f vel) + { + mostRecentlyUpdatedVelocityType = CDVEL_GpsNed; + hasVelocityGpsNed = true; + velocityGpsNed = vel; + } + + void setVelocityGpsEcef(vec3f vel) + { + mostRecentlyUpdatedVelocityType = CDVEL_GpsEcef; + hasVelocityGpsEcef = true; + velocityGpsEcef = vel; + } + + void setVelocityEstimatedNed(vec3f vel) + { + mostRecentlyUpdatedVelocityType = CDVEL_EstimatedNed; + hasVelocityEstimatedNed = true; + velocityEstimatedNed = vel; + } + + void setVelocityEstimatedEcef(vec3f vel) + { + mostRecentlyUpdatedVelocityType = CDVEL_EstimatedEcef; + hasVelocityEstimatedEcef = true; + velocityEstimatedEcef = vel; + } + + void setVelocityEstimatedBody(vec3f vel) + { + mostRecentlyUpdatedVelocityType = CDVEL_EstimatedBody; + hasVelocityEstimatedBody = true; + velocityEstimatedBody = vel; + } + + void setDeltaTime(float time) + { + hasDeltaTime = true; + deltaTime = time; + } + + void setDeltaTheta(vec3f theta) + { + hasDeltaTheta = true; + deltaTheta = theta; + } + + void setDeltaVelocity(vec3f vel) + { + hasDeltaVelocity = true; + deltaVelocity = vel; + } + + void setTimeStartup(uint64_t ts) + { + hasTimeStartup = true; + timeStartup = ts; + } + + void setTimeGps(uint64_t time) + { + hasTimeGps = true; + timeGps = time; + } + + void setTow(double t) + { + hasTow = true; + tow = t; + } + + void setWeek(uint16_t w) + { + hasWeek = true; + week = w; + } + + void setNumSats(uint8_t s) + { + hasNumSats = true; + numSats = s; + } + + void setTimeSyncIn(uint64_t t) + { + hasTimeSyncIn = true; + timeSyncIn = t; + } + + void setVpeStatus(VpeStatus s) + { + hasVpeStatus = true; + vpeStatus = s; + } + + void setInsStatus(InsStatus s) + { + hasInsStatus = true; + insStatus = s; + } + + void setSyncInCnt(uint32_t count) + { + hasSyncInCnt = true; + syncInCnt = count; + } + + void setTimeGpsPps(uint64_t pps) + { + hasTimeGpsPps = true; + timeGpsPps = pps; + } + + void setGpsTow(uint64_t tow) + { + hasGpsTow = true; + gpsTow = tow; + } + + void setPositionUncertaintyGpsNed(vec3f u) + { + mostRecentlyUpdatedPositionUncertaintyType = CDPOU_GpsNed; + hasPositionUncertaintyGpsNed = true; + positionUncertaintyGpsNed = u; + } + + void setPositionUncertaintyGpsEcef(vec3f u) + { + mostRecentlyUpdatedPositionUncertaintyType = CDPOU_GpsEcef; + hasPositionUncertaintyGpsEcef = true; + positionUncertaintyGpsEcef = u; + } + + void setPositionUncertaintyEstimated(float u) + { + mostRecentlyUpdatedPositionUncertaintyType = CDPOU_Estimated; + hasPositionUncertaintyEstimated = true; + positionUncertaintyEstimated = u; + } + + void setVelocityUncertaintyGps(float u) + { + mostRecentlyUpdatedVelocityUncertaintyType = CDVEU_Gps; + hasVelocityUncertaintyGps = true; + velocityUncertaintyGps = u; + } + + void setVelocityUncertaintyEstimated(float u) + { + mostRecentlyUpdatedVelocityUncertaintyType = CDVEU_Estimated; + hasVelocityUncertaintyEstimated = true; + velocityUncertaintyEstimated = u; + } + + void setTimeUncertainty(uint32_t u) + { + hasTimeUncertainty = true; + timeUncertainty = u; + } + + void setAttitudeUncertainty(vec3f u) + { + hasAttitudeUncertainty = true; + attitudeUncertainty = u; + } + + + void setFix(GpsFix f) + { + hasFix = true; + fix = f; + } + + void setTimeUtc(TimeUtc t) + { + hasTimeUtc = true; + timeUtc = t; + } + + void setSensSat(SensSat s) + { + hasSensSat = true; + sensSat = s; + } +}; + +CompositeData::CompositeData() : + _i(new Impl) +{ + _i->reset(); +} + +bool CompositeData::hasYawPitchRoll() +{ + return _i->hasYawPitchRoll; +} + +vec3f CompositeData::yawPitchRoll() +{ + if (!hasYawPitchRoll()) + throw invalid_operation(); + + return _i->yawPitchRoll; +} + +bool CompositeData::hasQuaternion() +{ + return _i->hasQuaternion; +} + +vec4f CompositeData::quaternion() +{ + if (!hasQuaternion()) + throw invalid_operation(); + + return _i->quaternion; +} + +bool CompositeData::hasDirectionCosineMatrix() +{ + return _i->hasDirectionCosineMatrix; +} + +mat3f CompositeData::directionCosineMatrix() +{ + if (!hasDirectionCosineMatrix()) + throw invalid_operation(); + + return _i->directionConsineMatrix; +} + +bool CompositeData::hasAnyMagnetic() +{ + return _i->mostRecentlyUpdatedMagneticType != Impl::CDMAG_None; +} + +vec3f CompositeData::anyMagnetic() +{ + switch (_i->mostRecentlyUpdatedMagneticType) + { + case Impl::CDMAG_None: + throw invalid_operation(); + case Impl::CDMAG_Normal: + return _i->magnetic; + case Impl::CDMAG_Uncompensated: + return _i->magneticUncompensated; + case Impl::CDMAG_Ned: + return _i->magneticNed; + case Impl::CDMAG_Ecef: + return _i->magneticEcef; + default: + throw not_implemented(); + } +} + +bool CompositeData::hasMagnetic() +{ + return _i->hasMagnetic; +} + +vec3f CompositeData::magnetic() +{ + if (!hasMagnetic()) + throw invalid_operation(); + + return _i->magnetic; +} + +bool CompositeData::hasMagneticUncompensated() +{ + return _i->hasMagneticUncompensated; +} + +vec3f CompositeData::magneticUncompensated() +{ + if (!hasMagneticUncompensated()) + throw invalid_operation(); + + return _i->magneticUncompensated; +} + +bool CompositeData::hasMagneticNed() +{ + return _i->hasMagneticNed; +} + +vec3f CompositeData::magneticNed() +{ + if (!hasMagneticNed()) + throw invalid_operation(); + + return _i->magneticNed; +} + +bool CompositeData::hasMagneticEcef() +{ + return _i->hasMagneticEcef; +} + +vec3f CompositeData::magneticEcef() +{ + if (!hasMagneticEcef()) + throw invalid_operation(); + + return _i->magneticEcef; +} + + +bool CompositeData::hasAnyAcceleration() +{ + return _i->mostRecentlyUpdatedAccelerationType != Impl::CDACC_None; +} + +vec3f CompositeData::anyAcceleration() +{ + switch (_i->mostRecentlyUpdatedAccelerationType) + { + case Impl::CDACC_None: + throw invalid_operation(); + case Impl::CDACC_Normal: + return _i->acceleration; + case Impl::CDACC_LinearBody: + return _i->accelerationLinearBody; + case Impl::CDACC_Uncompensated: + return _i->accelerationUncompensated; + case Impl::CDACC_LinearNed: + return _i->accelerationLinearNed; + case Impl::CDACC_Ned: + return _i->accelerationNed; + case Impl::CDACC_Ecef: + return _i->accelerationEcef; + case Impl::CDACC_LinearEcef: + return _i->accelerationLinearEcef; + default: + throw not_implemented(); + } +} + +bool CompositeData::hasAcceleration() +{ + return _i->hasAcceleration; +} + +vec3f CompositeData::acceleration() +{ + if (!hasAcceleration()) + throw invalid_operation(); + + return _i->acceleration; +} + +bool CompositeData::hasAccelerationLinearBody() +{ + return _i->hasAccelerationLinearBody; +} + +vec3f CompositeData::accelerationLinearBody() +{ + if (!hasAccelerationLinearBody()) + throw invalid_operation(); + + return _i->accelerationLinearBody; +} + +bool CompositeData::hasAccelerationUncompenated() +{ + return _i->hasAccelerationUncompensated; +} + +vec3f CompositeData::accelerationUncompensated() +{ + if (!hasAccelerationUncompenated()) + throw invalid_operation(); + + return _i->accelerationUncompensated; +} + +bool CompositeData::hasAccelerationLinearNed() +{ + return _i->hasAccelerationLinearNed; +} + +vec3f CompositeData::accelerationLinearNed() +{ + if (!hasAccelerationLinearNed()) + throw invalid_operation(); + + return _i->accelerationLinearNed; +} + +bool CompositeData::hasAccelerationLinearEcef() +{ + return _i->hasAccelerationLinearEcef; +} + +vec3f CompositeData::accelerationLinearEcef() +{ + if (!hasAccelerationLinearEcef()) + throw invalid_operation(); + + return _i->accelerationLinearEcef; +} + +bool CompositeData::hasAccelerationNed() +{ + return _i->hasAccelerationNed; +} + +vec3f CompositeData::accelerationNed() +{ + if (!hasAccelerationNed()) + throw invalid_operation(); + + return _i->accelerationNed; +} + +bool CompositeData::hasAccelerationEcef() +{ + return _i->hasAccelerationEcef; +} + +vec3f CompositeData::accelerationEcef() +{ + if (!hasAccelerationEcef()) + throw invalid_operation(); + + return _i->accelerationEcef; +} + + +bool CompositeData::hasAnyAngularRate() +{ + return _i->mostRecentlyUpdatedAngularRateType != Impl::CDANR_None; +} + +vec3f CompositeData::anyAngularRate() +{ + switch (_i->mostRecentlyUpdatedAngularRateType) + { + case Impl::CDANR_None: + throw invalid_operation(); + case Impl::CDANR_Normal: + return _i->angularRate; + case Impl::CDANR_Uncompensated: + return _i->angularRateUncompensated; + default: + throw not_implemented(); + } +} + +bool CompositeData::hasAngularRate() +{ + return _i->hasAngularRate; +} + +vec3f CompositeData::angularRate() +{ + if (!hasAngularRate()) + throw invalid_operation(); + + return _i->angularRate; +} + +bool CompositeData::hasAngularRateUncompensated() +{ + return _i->hasAngularRateUncompensated; +} + +vec3f CompositeData::angularRateUncompensated() +{ + if (!hasAngularRateUncompensated()) + throw invalid_operation(); + + return _i->angularRateUncompensated; +} + + +bool CompositeData::hasAnyTemperature() +{ + return _i->mostRecentlyUpdatedTemperatureType != Impl::CDTEM_None; +} + +float CompositeData::anyTemperature() +{ + switch (_i->mostRecentlyUpdatedTemperatureType) + { + case Impl::CDTEM_None: + throw invalid_operation(); + case Impl::CDTEM_Normal: + return _i->temperature; + default: + throw not_implemented(); + } +} + +bool CompositeData::hasTemperature() +{ + return _i->hasTemperature; +} + +float CompositeData::temperature() +{ + if (!hasTemperature()) + throw invalid_operation(); + + return _i->temperature; +} + + +bool CompositeData::hasAnyPressure() +{ + return _i->mostRecentlyUpdatePressureType != Impl::CDPRE_None; +} + +float CompositeData::anyPressure() +{ + switch (_i->mostRecentlyUpdatePressureType) + { + case Impl::CDPRE_None: + throw invalid_operation(); + case Impl::CDPRE_Normal: + return _i->pressure; + default: + throw not_implemented(); + } +} + +bool CompositeData::hasPressure() +{ + return _i->hasPressure; +} + +float CompositeData::pressure() +{ + if (!hasPressure()) + throw invalid_operation(); + + return _i->pressure; +} + +bool CompositeData::hasAnyPosition() +{ + return _i->mostRecentlyUpdatedPositionType != Impl::CDPOS_None; +} + +PositionD CompositeData::anyPosition() +{ + switch (_i->mostRecentlyUpdatedPositionType) + { + case Impl::CDPOS_None: + throw invalid_operation(); + case Impl::CDPOS_GpsLla: + return PositionD::fromLla(_i->positionGpsLla); + case Impl::CDPOS_GpsEcef: + return PositionD::fromEcef(_i->positionGpsEcef); + case Impl::CDPOS_EstimatedLla: + return PositionD::fromLla(_i->positionEstimatedLla); + case Impl::CDPOS_EstimatedEcef: + return PositionD::fromEcef(_i->positionEstimatedEcef); + default: + throw not_implemented(); + } +} + +bool CompositeData::hasPositionGpsLla() +{ + return _i->hasPositionGpsLla; +} + +vec3d CompositeData::positionGpsLla() +{ + if (!hasPositionGpsLla()) + throw invalid_operation(); + + return _i->positionGpsLla; +} + +bool CompositeData::hasPositionGpsEcef() +{ + return _i->hasPositionGpsEcef; +} + +vec3d CompositeData::positionGpsEcef() +{ + if (!hasPositionGpsEcef()) + throw invalid_operation(); + + return _i->positionGpsEcef; +} + +bool CompositeData::hasPositionEstimatedLla() +{ + return _i->hasPositionEstimatedLla; +} + +vec3d CompositeData::positionEstimatedLla() +{ + if (!hasPositionEstimatedLla()) + throw invalid_operation(); + + return _i->positionEstimatedLla; +} + +bool CompositeData::hasPositionEstimatedEcef() +{ + return _i->hasPositionEstimatedEcef; +} + +vec3d CompositeData::positionEstimatedEcef() +{ + if (!hasPositionEstimatedEcef()) + throw invalid_operation(); + + return _i->positionEstimatedEcef; +} + +bool CompositeData::hasAnyVelocity() +{ + return _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_None; +} + +vec3f CompositeData::anyVelocity() +{ + switch (_i->mostRecentlyUpdatedVelocityType) + { + case Impl::CDVEL_None: + throw invalid_operation(); + case Impl::CDVEL_GpsNed: + return _i->velocityGpsNed; + case Impl::CDVEL_GpsEcef: + return _i->velocityGpsEcef; + case Impl::CDVEL_EstimatedNed: + return _i->velocityEstimatedNed; + case Impl::CDVEL_EstimatedEcef: + return _i->velocityEstimatedEcef; + case Impl::CDVEL_EstimatedBody: + return _i->velocityEstimatedBody; + default: + throw not_implemented(); + } +} + +bool CompositeData::hasVelocityGpsNed() +{ + return _i->hasVelocityGpsNed; +} + +vec3f CompositeData::velocityGpsNed() +{ + if (!hasVelocityGpsNed()) + throw invalid_operation(); + + return _i->velocityGpsNed; +} + +bool CompositeData::hasVelocityGpsEcef() +{ + return _i->hasVelocityGpsEcef; +} + +vec3f CompositeData::velocityGpsEcef() +{ + if (!hasVelocityGpsEcef()) + throw invalid_operation(); + + return _i->velocityGpsEcef; +} + +bool CompositeData::hasVelocityEstimatedNed() +{ + return _i->hasVelocityEstimatedNed; +} + +vec3f CompositeData::velocityEstimatedNed() +{ + if (!hasVelocityEstimatedNed()) + throw invalid_operation(); + + return _i->velocityEstimatedNed; +} + +bool CompositeData::hasVelocityEstimatedEcef() +{ + return _i->hasVelocityEstimatedEcef; +} + +vec3f CompositeData::velocityEstimatedEcef() +{ + if (!hasVelocityEstimatedEcef()) + throw invalid_operation(); + + return _i->velocityEstimatedEcef; +} + +bool CompositeData::hasVelocityEstimatedBody() +{ + return _i->hasVelocityEstimatedBody; +} + +vec3f CompositeData::velocityEstimatedBody() +{ + if (!hasVelocityEstimatedBody()) + throw invalid_operation(); + + return _i->velocityEstimatedBody; +} + +bool CompositeData::hasDeltaTime() +{ + return _i->hasDeltaTime; +} + +float CompositeData::deltaTime() +{ + if (!hasDeltaTime()) + throw invalid_operation(); + + return _i->deltaTime; +} + +bool CompositeData::hasDeltaTheta() +{ + return _i->hasDeltaTheta; +} + +vec3f CompositeData::deltaTheta() +{ + if (!hasDeltaTheta()) + throw invalid_operation(); + + return _i->deltaTheta; +} + +bool CompositeData::hasDeltaVelocity() +{ + return _i->hasDeltaVelocity; +} + +vec3f CompositeData::deltaVelocity() +{ + if (!hasDeltaVelocity()) + throw invalid_operation(); + + return _i->deltaVelocity; +} + +bool CompositeData::hasTimeStartup() +{ + return _i->hasTimeStartup; +} + +uint64_t CompositeData::timeStartup() +{ + if (!hasTimeStartup()) + throw invalid_operation(); + + return _i->timeStartup; +} + +bool CompositeData::hasTimeGps() +{ + return _i->hasTimeGps; +} + +uint64_t CompositeData::timeGps() +{ + if (!hasTimeGps()) + throw invalid_operation(); + + return _i->timeGps; +} + +bool CompositeData::hasTow() +{ + return _i->hasTow; +} + +double CompositeData::tow() +{ + if (!hasTow()) + throw invalid_operation(); + + return _i->tow; +} + +bool CompositeData::hasWeek() +{ + return _i->hasWeek; +} + +uint16_t CompositeData::week() +{ + if (!hasWeek()) + throw invalid_operation(); + + return _i->week; +} + +bool CompositeData::hasNumSats() +{ + return _i->hasNumSats; +} + +uint8_t CompositeData::numSats() +{ + if (!hasNumSats()) + throw invalid_operation(); + + return _i->numSats; +} + +bool CompositeData::hasTimeSyncIn() +{ + return _i->hasTimeSyncIn; +} + +uint64_t CompositeData::timeSyncIn() +{ + if (!hasTimeSyncIn()) + throw invalid_operation(); + + return _i->timeSyncIn; +} + +bool CompositeData::hasVpeStatus() +{ + return _i->hasVpeStatus; +} + +VpeStatus CompositeData::vpeStatus() +{ + if (!hasVpeStatus()) + throw invalid_operation(); + + return _i->vpeStatus; +} + +bool CompositeData::hasInsStatus() +{ + return _i->hasInsStatus; +} + +InsStatus CompositeData::insStatus() +{ + if (!hasInsStatus()) + throw invalid_operation(); + + return _i->insStatus; +} + +bool CompositeData::hasSyncInCnt() +{ + return _i->hasSyncInCnt; +} + +uint32_t CompositeData::syncInCnt() +{ + if (!hasSyncInCnt()) + throw invalid_operation(); + + return _i->syncInCnt; +} + +bool CompositeData::hasTimeGpsPps() +{ + return _i->hasTimeGpsPps; +} + +uint64_t CompositeData::timeGpsPps() +{ + if (!hasTimeGpsPps()) + throw invalid_operation(); + + return _i->timeGpsPps; +} + +bool CompositeData::hasGpsTow() +{ + return _i->hasGpsTow; +} + +uint64_t CompositeData::gpsTow() +{ + if (!hasGpsTow()) + throw invalid_operation(); + + return _i->gpsTow; +} + +bool CompositeData::hasTimeUtc() +{ + return _i->hasTimeUtc; +} + +TimeUtc CompositeData::timeUtc() +{ + if (!hasTimeUtc()) + throw invalid_operation(); + + return _i->timeUtc; +} + +bool CompositeData::hasSensSat() +{ + return _i->hasSensSat; +} + +SensSat CompositeData::sensSat() +{ + if (!hasSensSat()) + throw invalid_operation(); + + return _i->sensSat; +} + +bool CompositeData::hasFix() +{ + return _i->hasFix; +} + +GpsFix CompositeData::fix() +{ + if (!hasFix()) + throw invalid_operation(); + + return _i->fix; +} + +bool CompositeData::hasAnyPositionUncertainty() +{ + return _i->mostRecentlyUpdatedPositionUncertaintyType != Impl::CDPOU_None; +} + +vec3f CompositeData::anyPositionUncertainty() +{ + switch (_i->mostRecentlyUpdatedPositionUncertaintyType) + { + case Impl::CDPOU_None: + throw invalid_operation(); + case Impl::CDPOU_GpsNed: + return _i->positionUncertaintyGpsNed; + case Impl::CDPOU_GpsEcef: + return _i->positionUncertaintyGpsEcef; + case Impl::CDPOU_Estimated: + return vec3f(_i->positionUncertaintyEstimated); + default: + throw not_implemented(); + } +} + +bool CompositeData::hasPositionUncertaintyGpsNed() +{ + return _i->hasPositionUncertaintyGpsNed; +} + +vec3f CompositeData::positionUncertaintyGpsNed() +{ + if (!hasPositionUncertaintyGpsNed()) + throw invalid_operation(); + + return _i->positionUncertaintyGpsNed; +} + +bool CompositeData::hasPositionUncertaintyGpsEcef() +{ + return _i->hasPositionUncertaintyGpsEcef; +} + +vec3f CompositeData::positionUncertaintyGpsEcef() +{ + if (!hasPositionUncertaintyGpsEcef()) + throw invalid_operation(); + + return _i->positionUncertaintyGpsEcef; +} + +bool CompositeData::hasPositionUncertaintyEstimated() +{ + return _i->hasPositionUncertaintyEstimated; +} + +float CompositeData::positionUncertaintyEstimated() +{ + if (!hasPositionUncertaintyEstimated()) + throw invalid_operation(); + + return _i->positionUncertaintyEstimated; +} + +bool CompositeData::hasAnyVelocityUncertainty() +{ + return _i->mostRecentlyUpdatedVelocityUncertaintyType != Impl::CDVEU_None; +} + +float CompositeData::anyVelocityUncertainty() +{ + switch (_i->mostRecentlyUpdatedVelocityUncertaintyType) + { + case Impl::CDVEU_None: + throw invalid_operation(); + case Impl::CDVEU_Gps: + return _i->velocityUncertaintyGps; + case Impl::CDVEU_Estimated: + return _i->velocityUncertaintyEstimated; + default: + throw not_implemented(); + } +} + +bool CompositeData::hasVelocityUncertaintyGps() +{ + return _i->hasVelocityUncertaintyGps; +} + +float CompositeData::velocityUncertaintyGps() +{ + if (!hasVelocityUncertaintyGps()) + throw invalid_operation(); + + return _i->velocityUncertaintyGps; +} + +bool CompositeData::hasVelocityUncertaintyEstimated() +{ + return _i->hasVelocityUncertaintyEstimated; +} + +float CompositeData::velocityUncertaintyEstimated() +{ + if (!hasVelocityUncertaintyEstimated()) + throw invalid_operation(); + + return _i->velocityUncertaintyEstimated; +} + +bool CompositeData::hasTimeUncertainty() +{ + return _i->hasTimeUncertainty; +} + +uint32_t CompositeData::timeUncertainty() +{ + if (!hasTimeUncertainty()) + throw invalid_operation(); + + return _i->timeUncertainty; +} + +bool CompositeData::hasAttitudeUncertainty() +{ + return _i->hasAttitudeUncertainty; +} + +vec3f CompositeData::attitudeUncertainty() +{ + if (!hasAttitudeUncertainty()) + throw invalid_operation(); + + return _i->attitudeUncertainty; +} + +bool CompositeData::hasCourseOverGround() +{ + return _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_None + && _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_EstimatedBody // TODO: Don't have conversion formula from body frame to NED frame. + && _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_EstimatedEcef // TODO: Don't have conversion formula from ECEF frame to NED frame. + && _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_GpsEcef; // TODO: Don't have conversion formula from ECEF frame to NED frame. +} + +float CompositeData::courseOverGround() +{ + if (!hasCourseOverGround()) + throw invalid_operation(); + + switch (_i->mostRecentlyUpdatedVelocityType) + { + case Impl::CDVEL_GpsNed: + return course_over_ground(_i->velocityGpsNed); + case Impl::CDVEL_EstimatedNed: + return course_over_ground(_i->velocityEstimatedNed); + default: + throw not_implemented(); + } +} + +bool CompositeData::hasSpeedOverGround() +{ + return _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_None + && _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_EstimatedBody // TODO: Don't have conversion formula from body frame to NED frame. + && _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_EstimatedEcef // TODO: Don't have conversion formula from ECEF frame to NED frame. + && _i->mostRecentlyUpdatedVelocityType != Impl::CDVEL_GpsEcef; // TODO: Don't have conversion formula from ECEF frame to NED frame. +} + +float CompositeData::speedOverGround() +{ + if (!hasSpeedOverGround()) + throw invalid_operation(); + + switch (_i->mostRecentlyUpdatedVelocityType) + { + case Impl::CDVEL_GpsNed: + return speed_over_ground(_i->velocityGpsNed); + case Impl::CDVEL_EstimatedNed: + return speed_over_ground(_i->velocityEstimatedNed); + default: + throw not_implemented(); + } +} + + +CompositeData CompositeData::parse(Packet& p) +{ + CompositeData o; + + parse(p, o); + + return o; +} + +void CompositeData::parse(Packet& p, CompositeData& o) +{ + vector v; + v.push_back(&o); + + parse(p, v); +} + +void CompositeData::parse(Packet& p, vector& o) +{ + if (p.type() == Packet::TYPE_ASCII) + parseAscii(p, o); + else if (p.type() == Packet::TYPE_BINARY) + parseBinary(p, o); + else + throw not_supported(); +} + +void CompositeData::reset() +{ + _i->reset(); +} + +bool CompositeData::hasAnyAttitude() +{ + return _i->mostRecentlyUpdatedAttitudeType != Impl::CDATT_None; +} + +AttitudeF CompositeData::anyAttitude() +{ + switch (_i->mostRecentlyUpdatedAttitudeType) + { + case Impl::CDATT_None: + throw invalid_operation(); + case Impl::CDATT_YawPitchRoll: + return AttitudeF::fromYprInDegs(_i->yawPitchRoll); + case Impl::CDATT_Quaternion: + return AttitudeF::fromQuat(_i->quaternion); + case Impl::CDATT_DirectionCosineMatrix: + return AttitudeF::fromDcm(_i->directionConsineMatrix); + default: + throw not_implemented(); + } +} + +void CompositeData::parseAscii(Packet& p, vector& o) +{ + switch (p.determineAsciiAsyncType()) + { + + case VNYPR: + { + vec3f ypr; + + p.parseVNYPR(&ypr); + + for (cditer i = o.begin(); i != o.end(); ++i) + (*i)->_i->setYawPitchRoll(ypr); + + break; + } + + case VNQTN: + { + vec4f quat; + + p.parseVNQTN(&quat); + + for (cditer i = o.begin(); i != o.end(); ++i) + (*i)->_i->setQuaternion(quat); + + break; + } + + + case VNQMR: + { + vec4f quat; + vec3f mag, accel, ar; + + p.parseVNQMR(&quat, &mag, &accel, &ar); + + for (cditer i = o.begin(); i != o.end(); ++i) + { + (*i)->_i->setQuaternion(quat); + (*i)->_i->setMagnetic(mag); + (*i)->_i->setAcceleration(accel); + (*i)->_i->setAngularRate(ar); + } + + break; + } + + + case VNMAG: + { + vec3f mag; + + p.parseVNMAG(&mag); + + for (cditer i = o.begin(); i != o.end(); ++i) + (*i)->_i->setMagnetic(mag); + + break; + } + + case VNACC: + { + vec3f accel; + + p.parseVNACC(&accel); + + for (cditer i = o.begin(); i != o.end(); ++i) + (*i)->_i->setAcceleration(accel); + + break; + } + + case VNGYR: + { + vec3f ar; + + p.parseVNGYR(&ar); + + for (cditer i = o.begin(); i != o.end(); ++i) + (*i)->_i->setAngularRate(ar); + + break; + } + + case VNMAR: + { + vec3f mag, accel, ar; + + p.parseVNMAR(&mag, &accel, &ar); + + for (cditer i = o.begin(); i != o.end(); ++i) + { + (*i)->_i->setMagnetic(mag); + (*i)->_i->setAcceleration(accel); + (*i)->_i->setAngularRate(ar); + } + + break; + } + + case VNYMR: + { + vec3f ypr, mag, accel, ar; + + p.parseVNYMR(&ypr, &mag, &accel, &ar); + + for (cditer i = o.begin(); i != o.end(); ++i) + { + (*i)->_i->setYawPitchRoll(ypr); + (*i)->_i->setMagnetic(mag); + (*i)->_i->setAcceleration(accel); + (*i)->_i->setAngularRate(ar); + } + + break; + } + + + case VNYBA: + { + vec3f ypr, accel, ar; + + p.parseVNYBA(&ypr, &accel, &ar); + + for (cditer i = o.begin(); i != o.end(); ++i) + { + (*i)->_i->setYawPitchRoll(ypr); + (*i)->_i->setAccelerationLinearBody(accel); + (*i)->_i->setAngularRate(ar); + } + + break; + } + + case VNYIA: + { + vec3f ypr, accel, ar; + + p.parseVNYIA(&ypr, &accel, &ar); + + for (cditer i = o.begin(); i != o.end(); ++i) + { + (*i)->_i->setYawPitchRoll(ypr); + (*i)->_i->setAccelerationLinearNed(accel); + (*i)->_i->setAngularRate(ar); + } + + break; + } + + + case VNIMU: + { + vec3f accel, ar, mag; + float temp, pres; + + p.parseVNIMU(&mag, &accel, &ar, &temp, &pres); + + for (cditer i = o.begin(); i != o.end(); ++i) + { + (*i)->_i->setMagneticUncompensated(mag); + (*i)->_i->setAccelerationUncompensated(accel); + (*i)->_i->setAngularRateUncompensated(ar); + (*i)->_i->setTemperature(temp); + (*i)->_i->setPressure(pres); + } + + break; + } + + case VNGPS: + { + double time; + uint16_t week; + uint8_t fix, numSats; + vec3d lla; + vec3f nedVel, nedAcc; + float speedAcc, timeAcc; + + p.parseVNGPS(&time, &week, &fix, &numSats, &lla, &nedVel, &nedAcc, &speedAcc, &timeAcc); + + for (cditer i = o.begin(); i != o.end(); ++i) + { + (*i)->_i->setTow(time); + (*i)->_i->setWeek(week); + (*i)->_i->setFix(static_cast(fix)); + (*i)->_i->setNumSats(numSats); + (*i)->_i->setPositionGpsLla(lla); + (*i)->_i->setVelocityGpsNed(nedVel); + (*i)->_i->setPositionUncertaintyGpsNed(nedAcc); + (*i)->_i->setVelocityUncertaintyGps(speedAcc); + // Convert to uint32_t since this is the binary representation in nanoseconds. + (*i)->_i->setTimeUncertainty(static_cast(timeAcc * 1e9)); + } + + break; + } + + case VNGPE: + { + double tow; + uint16_t week; + uint8_t fix, numSats; + vec3d position; + vec3f ecefVel, ecefAcc; + float speedAcc, timeAcc; + + p.parseVNGPE(&tow, &week, &fix, &numSats, &position, &ecefVel, &ecefAcc, &speedAcc, &timeAcc); + + for (cditer i = o.begin(); i != o.end(); ++i) + { + (*i)->_i->setTow(tow); + (*i)->_i->setWeek(week); + (*i)->_i->setFix(static_cast(fix)); + (*i)->_i->setNumSats(numSats); + (*i)->_i->setPositionGpsEcef(position); + (*i)->_i->setVelocityGpsEcef(ecefVel); + (*i)->_i->setPositionUncertaintyGpsEcef(ecefAcc); + (*i)->_i->setVelocityUncertaintyGps(speedAcc); + // Convert to uint32_t since this is the binary representation in nanoseconds. + (*i)->_i->setTimeUncertainty(static_cast(timeAcc * 1e9)); + } + + break; + } + + case VNINS: + { + double tow; + uint16_t week, status; + vec3d position; + vec3f ypr, nedVel; + float attUncertainty, velUncertainty, posUncertainty; + + p.parseVNINS(&tow, &week, &status, &ypr, &position, &nedVel, &attUncertainty, &posUncertainty, &velUncertainty); + + for (cditer i = o.begin(); i != o.end(); ++i) + { + (*i)->_i->setTow(tow); + (*i)->_i->setWeek(week); + (*i)->_i->setInsStatus(static_cast(status)); + (*i)->_i->setYawPitchRoll(ypr); + (*i)->_i->setPositionEstimatedLla(position); + (*i)->_i->setVelocityEstimatedNed(nedVel); + // Binary data provides 3 components to yaw, pitch, roll uncertainty. + (*i)->_i->setAttitudeUncertainty(vec3f(attUncertainty)); + (*i)->_i->setPositionUncertaintyEstimated(posUncertainty); + (*i)->_i->setVelocityUncertaintyEstimated(velUncertainty); + } + + break; + } + + case VNINE: + { + double tow; + uint16_t week, status; + vec3d position; + vec3f ypr, velocity; + float attUncertainty, velUncertainty, posUncertainty; + + p.parseVNINE(&tow, &week, &status, &ypr, &position, &velocity, &attUncertainty, &posUncertainty, &velUncertainty); + + for (cditer i = o.begin(); i != o.end(); ++i) + { + (*i)->_i->setTow(tow); + (*i)->_i->setWeek(week); + (*i)->_i->setInsStatus(static_cast(status)); + (*i)->_i->setYawPitchRoll(ypr); + (*i)->_i->setPositionEstimatedEcef(position); + (*i)->_i->setVelocityEstimatedEcef(velocity); + // Binary data provides 3 components to yaw, pitch, roll uncertainty. + (*i)->_i->setAttitudeUncertainty(vec3f(attUncertainty)); + (*i)->_i->setPositionUncertaintyEstimated(posUncertainty); + (*i)->_i->setVelocityUncertaintyEstimated(velUncertainty); + } + + break; + } + + case VNISL: + { + vec3f ypr, velocity, accel, ar; + vec3d lla; + + p.parseVNISL(&ypr, &lla, &velocity, &accel, &ar); + + for (cditer i = o.begin(); i != o.end(); ++i) + { + (*i)->_i->setYawPitchRoll(ypr); + (*i)->_i->setPositionEstimatedLla(lla); + (*i)->_i->setVelocityEstimatedNed(velocity); + (*i)->_i->setAcceleration(accel); + (*i)->_i->setAngularRate(ar); + } + + break; + } + + case VNISE: + { + vec3f ypr, velocity, accel, ar; + vec3d position; + + p.parseVNISE(&ypr, &position, &velocity, &accel, &ar); + + for (cditer i = o.begin(); i != o.end(); ++i) + { + (*i)->_i->setYawPitchRoll(ypr); + (*i)->_i->setPositionEstimatedEcef(position); + (*i)->_i->setVelocityEstimatedEcef(velocity); + (*i)->_i->setAcceleration(accel); + (*i)->_i->setAngularRate(ar); + } + + break; + } + + case VNDTV: + { + float deltaTime; + vec3f deltaTheta, deltaVel; + + p.parseVNDTV(&deltaTime, &deltaTheta, &deltaVel); + + for (cditer i = o.begin(); i != o.end(); ++i) + { + (*i)->_i->setDeltaTime(deltaTime); + (*i)->_i->setDeltaTheta(deltaTheta); + (*i)->_i->setDeltaVelocity(deltaVel); + } + + break; + } + + + default: + throw not_supported(); + + } +} + +void CompositeData::parseBinary(Packet& p, vector& o) +{ + BinaryGroup groups = static_cast(p.groups()); + size_t curGroupFieldIndex = 0; + + if (groups & BINARYGROUP_COMMON) + parseBinaryPacketCommonGroup(p, CommonGroup(p.groupField(curGroupFieldIndex++)), o); + if (groups & BINARYGROUP_TIME) + parseBinaryPacketTimeGroup(p, TimeGroup(p.groupField(curGroupFieldIndex++)), o); + if (groups & BINARYGROUP_IMU) + parseBinaryPacketImuGroup(p, ImuGroup(p.groupField(curGroupFieldIndex++)), o); + if (groups & BINARYGROUP_GPS) + parseBinaryPacketGpsGroup(p, GpsGroup(p.groupField(curGroupFieldIndex++)), o); + if (groups & BINARYGROUP_ATTITUDE) + parseBinaryPacketAttitudeGroup(p, AttitudeGroup(p.groupField(curGroupFieldIndex++)), o); + if (groups & BINARYGROUP_INS) + parseBinaryPacketInsGroup(p, InsGroup(p.groupField(curGroupFieldIndex++)), o); +} + +void CompositeData::parseBinaryPacketCommonGroup(Packet& p, CommonGroup gf, vector& o) +{ + if (gf & COMMONGROUP_TIMESTARTUP) + setValues(p.extractUint64(), o, &Impl::setTimeStartup); + + if (gf & COMMONGROUP_TIMEGPS) + setValues(p.extractUint64(), o, &Impl::setTimeGps); + + if (gf & COMMONGROUP_TIMESYNCIN) + setValues(p.extractUint64(), o, &Impl::setTimeSyncIn); + + if (gf & COMMONGROUP_YAWPITCHROLL) + setValues(p.extractVec3f(), o, &Impl::setYawPitchRoll); + + if (gf & COMMONGROUP_QUATERNION) + setValues(p.extractVec4f(), o, &Impl::setQuaternion); + + if (gf & COMMONGROUP_ANGULARRATE) + setValues(p.extractVec3f(), o, &Impl::setAngularRate); + + if (gf & COMMONGROUP_POSITION) + setValues(p.extractVec3d(), o, &Impl::setPositionEstimatedLla); + + if (gf & COMMONGROUP_VELOCITY) + setValues(p.extractVec3f(), o, &Impl::setVelocityEstimatedNed); + + if (gf & COMMONGROUP_ACCEL) + setValues(p.extractVec3f(), o, &Impl::setAcceleration); + + if (gf & COMMONGROUP_IMU) + { + setValues(p.extractVec3f(), o, &Impl::setAccelerationUncompensated); + setValues(p.extractVec3f(), o, &Impl::setAngularRateUncompensated); + } + + if (gf & COMMONGROUP_MAGPRES) + { + setValues(p.extractVec3f(), o, &Impl::setMagnetic); + setValues(p.extractFloat(), o, &Impl::setTemperature); + setValues(p.extractFloat(), o, &Impl::setPressure); + } + + if (gf & COMMONGROUP_DELTATHETA) + { + setValues(p.extractFloat(), o, &Impl::setDeltaTime); + setValues(p.extractVec3f(), o, &Impl::setDeltaTheta); + setValues(p.extractVec3f(), o, &Impl::setDeltaVelocity); + } + + if (gf & COMMONGROUP_INSSTATUS) + { + // Don't know if this is a VN-100, VN-200 or VN-300 so we can't know for sure if + // this is VpeStatus or InsStatus. + uint16_t v = p.extractUint16(); + setValues(VpeStatus(v), o, &Impl::setVpeStatus); + setValues(InsStatus(v), o, &Impl::setInsStatus); + } + + if (gf & COMMONGROUP_SYNCINCNT) + setValues(p.extractUint32(), o, &Impl::setSyncInCnt); + + if (gf & COMMONGROUP_TIMEGPSPPS) + setValues(p.extractUint64(), o, &Impl::setTimeGpsPps); +} + +void CompositeData::parseBinaryPacketTimeGroup(Packet& p, TimeGroup gf, vector& o) +{ + if (gf & TIMEGROUP_TIMESTARTUP) + setValues(p.extractUint64(), o, &Impl::setTimeStartup); + + if (gf & TIMEGROUP_TIMEGPS) + setValues(p.extractUint64(), o, &Impl::setTimeGps); + + if (gf & TIMEGROUP_GPSTOW) + setValues(p.extractUint64(), o, &Impl::setGpsTow); + + if (gf & TIMEGROUP_GPSWEEK) + setValues(p.extractUint16(), o, &Impl::setWeek); + + if (gf & TIMEGROUP_TIMESYNCIN) + setValues(p.extractUint64(), o, &Impl::setTimeSyncIn); + + if (gf & TIMEGROUP_TIMEGPSPPS) + setValues(p.extractUint64(), o, &Impl::setTimeGpsPps); + + if (gf & TIMEGROUP_TIMEUTC) + { + TimeUtc t; + + t.year = p.extractInt8(); + t.month = p.extractUint8(); + t.day = p.extractUint8(); + t.hour = p.extractUint8(); + t.min = p.extractUint8(); + t.sec = p.extractUint8(); + t.ms = p.extractUint8(); + + setValues(t, o, &Impl::setTimeUtc); + } + + if (gf & TIMEGROUP_SYNCINCNT) + setValues(p.extractUint32(), o, &Impl::setSyncInCnt); +} + +void CompositeData::parseBinaryPacketImuGroup(Packet& p, ImuGroup gf, vector& o) +{ + if (gf & IMUGROUP_IMUSTATUS) + // This field is currently reserved. + p.extractUint16(); + + if (gf & IMUGROUP_UNCOMPMAG) + setValues(p.extractVec3f(), o, &Impl::setMagneticUncompensated); + + if (gf & IMUGROUP_UNCOMPACCEL) + setValues(p.extractVec3f(), o, &Impl::setAccelerationUncompensated); + + if (gf & IMUGROUP_UNCOMPGYRO) + setValues(p.extractVec3f(), o, &Impl::setAngularRateUncompensated); + + if (gf & IMUGROUP_TEMP) + setValues(p.extractFloat(), o, &Impl::setTemperature); + + if (gf & IMUGROUP_PRES) + setValues(p.extractFloat(), o, &Impl::setPressure); + + if (gf & IMUGROUP_DELTATHETA) + { + setValues(p.extractFloat(), o, &Impl::setDeltaTime); + setValues(p.extractVec3f(), o, &Impl::setDeltaTheta); + } + + if (gf & IMUGROUP_DELTAVEL) + setValues(p.extractVec3f(), o, &Impl::setDeltaVelocity); + + if (gf & IMUGROUP_MAG) + setValues(p.extractVec3f(), o, &Impl::setMagnetic); + + if (gf & IMUGROUP_ACCEL) + setValues(p.extractVec3f(), o, &Impl::setAcceleration); + + if (gf & IMUGROUP_ANGULARRATE) + setValues(p.extractVec3f(), o, &Impl::setAngularRate); + + if (gf & IMUGROUP_SENSSAT) + setValues(SensSat(p.extractUint16()), o, &Impl::setSensSat); + +} + +void CompositeData::parseBinaryPacketGpsGroup(Packet& p, GpsGroup gf, vector& o) +{ + if (gf & GPSGROUP_UTC) + { + TimeUtc t; + + t.year = p.extractInt8(); + t.month = p.extractUint8(); + t.day = p.extractUint8(); + t.hour = p.extractUint8(); + t.min = p.extractUint8(); + t.sec = p.extractUint8(); + t.ms = p.extractUint8(); + + setValues(t, o, &Impl::setTimeUtc); + } + + if (gf & GPSGROUP_TOW) + setValues(p.extractUint64(), o, &Impl::setGpsTow); + + if (gf & GPSGROUP_WEEK) + setValues(p.extractUint16(), o, &Impl::setWeek); + + if (gf & GPSGROUP_NUMSATS) + setValues(p.extractUint8(), o, &Impl::setNumSats); + + if (gf & GPSGROUP_FIX) + setValues(GpsFix(p.extractUint8()), o, &Impl::setFix); + + if (gf & GPSGROUP_POSLLA) + setValues(p.extractVec3d(), o, &Impl::setPositionGpsLla); + + if (gf & GPSGROUP_POSECEF) + setValues(p.extractVec3d(), o, &Impl::setPositionGpsEcef); + + if (gf & GPSGROUP_VELNED) + setValues(p.extractVec3f(), o, &Impl::setVelocityGpsNed); + + if (gf & GPSGROUP_VELECEF) + setValues(p.extractVec3f(), o, &Impl::setVelocityGpsEcef); + + if (gf & GPSGROUP_POSU) + setValues(p.extractVec3f(), o, &Impl::setPositionUncertaintyGpsNed); + + if (gf & GPSGROUP_VELU) + setValues(p.extractFloat(), o, &Impl::setVelocityUncertaintyGps); + + if (gf & GPSGROUP_TIMEU) + setValues(p.extractUint32(), o, &Impl::setTimeUncertainty); + +} + +void CompositeData::parseBinaryPacketAttitudeGroup(Packet& p, AttitudeGroup gf, vector& o) +{ + if (gf & ATTITUDEGROUP_VPESTATUS) + setValues(VpeStatus(p.extractUint16()), o, &Impl::setVpeStatus); + + if (gf & ATTITUDEGROUP_YAWPITCHROLL) + setValues(p.extractVec3f(), o, &Impl::setYawPitchRoll); + + if (gf & ATTITUDEGROUP_QUATERNION) + setValues(p.extractVec4f(), o, &Impl::setQuaternion); + + if (gf & ATTITUDEGROUP_DCM) + setValues(p.extractMat3f(), o, &Impl::setDirectionConsineMatrix); + + if (gf & ATTITUDEGROUP_MAGNED) + setValues(p.extractVec3f(), o, &Impl::setMagneticNed); + + if (gf & ATTITUDEGROUP_ACCELNED) + setValues(p.extractVec3f(), o, &Impl::setAccelerationNed); + + if (gf & ATTITUDEGROUP_LINEARACCELBODY) + setValues(p.extractVec3f(), o, &Impl::setAccelerationLinearBody); + + if (gf & ATTITUDEGROUP_LINEARACCELNED) + setValues(p.extractVec3f(), o, &Impl::setAccelerationLinearNed); + + if (gf & ATTITUDEGROUP_YPRU) + setValues(p.extractVec3f(), o, &Impl::setAttitudeUncertainty); + +} + +void CompositeData::parseBinaryPacketInsGroup(Packet& p, InsGroup gf, vector& o) +{ + if (gf & INSGROUP_INSSTATUS) + setValues(InsStatus(p.extractUint16()), o, &Impl::setInsStatus); + + if (gf & INSGROUP_POSLLA) + setValues(p.extractVec3d(), o, &Impl::setPositionEstimatedLla); + + if (gf & INSGROUP_POSECEF) + setValues(p.extractVec3d(), o, &Impl::setPositionEstimatedEcef); + + if (gf & INSGROUP_VELBODY) + setValues(p.extractVec3f(), o, &Impl::setVelocityEstimatedBody); + + if (gf & INSGROUP_VELNED) + setValues(p.extractVec3f(), o, &Impl::setVelocityEstimatedNed); + + if (gf & INSGROUP_VELECEF) + setValues(p.extractVec3f(), o, &Impl::setVelocityEstimatedEcef); + + if (gf & INSGROUP_MAGECEF) + setValues(p.extractVec3f(), o, &Impl::setMagneticEcef); + + if (gf & INSGROUP_ACCELECEF) + setValues(p.extractVec3f(), o, &Impl::setAccelerationEcef); + + if (gf & INSGROUP_LINEARACCELECEF) + setValues(p.extractVec3f(), o, &Impl::setAccelerationLinearEcef); + + if (gf & INSGROUP_POSU) + setValues(p.extractFloat(), o, &Impl::setPositionUncertaintyEstimated); + + if (gf & INSGROUP_VELU) + setValues(p.extractFloat(), o, &Impl::setVelocityUncertaintyEstimated); + +} + +} +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/sensors/ezasyncdata.cpp b/ext/vnproglib-1.1.0.115/src/vn/sensors/ezasyncdata.cpp new file mode 100755 index 0000000..b662b1f --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/sensors/ezasyncdata.cpp @@ -0,0 +1,104 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/sensors/ezasyncdata.h" + +using namespace std; + +namespace vn { +namespace sensors { + +#if defined(_MSC_VER) + #pragma warning(push) + + // Disable warnings about unused parameters. + #pragma warning(disable:4100) +#endif + +void EzAsyncData::asyncPacketReceivedHandler(void* userData, protocol::uart::Packet& p, size_t index) +{ + EzAsyncData* ez = static_cast(userData); + + CompositeData nd; + + #if VN_SUPPORTS_INITIALIZER_LIST + vector d({ &ez->_persistentData, &nd }); + #else + vector d(2); + d[0] = &ez->_persistentData; + d[1] = &nd; + #endif + + ez->_mainCS.enter(); + CompositeData::parse(p, d); + ez->_mainCS.leave(); + + ez->_copyCS.enter(); + ez->_nextData = nd; + ez->_copyCS.leave(); + + ez->_newDataEvent.signal(); +} + +#if defined (_MSC_VER) + #pragma warning(pop) +#endif + +EzAsyncData::EzAsyncData(VnSensor* sensor) : + _sensor(sensor) +{ + _sensor->registerAsyncPacketReceivedHandler(this, &EzAsyncData::asyncPacketReceivedHandler); +} + +VnSensor* EzAsyncData::sensor() +{ + return _sensor; +} + +EzAsyncData* EzAsyncData::connect(string portName, uint32_t baudrate) +{ + VnSensor* s = new VnSensor(); + + s->connect(portName, baudrate); + + return new EzAsyncData(s); +} + +void EzAsyncData::disconnect() +{ + _sensor->unregisterAsyncPacketReceivedHandler(); + + _sensor->disconnect(); +} + +CompositeData EzAsyncData::currentData() +{ + CompositeData cd; + + _mainCS.enter(); + cd = _persistentData; + _mainCS.leave(); + + return cd; +} + +CompositeData EzAsyncData::getNextData() +{ + return getNextData(0xFFFFFFFF); +} + +CompositeData EzAsyncData::getNextData(int timeoutMs) +{ + if (_newDataEvent.waitMs(timeoutMs) == xplat::Event::WAIT_TIMEDOUT) + throw timeout(); + + CompositeData cd; + + _copyCS.enter(); + cd = _nextData; + _copyCS.leave(); + + return cd; +} + +} +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/sensors/python.cpp b/ext/vnproglib-1.1.0.115/src/vn/sensors/python.cpp new file mode 100755 index 0000000..6aa51b8 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/sensors/python.cpp @@ -0,0 +1,986 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/util/boostpython.h" + +#include "vn/sensors/sensors.h" +#include "vn/sensors/ezasyncdata.h" +#include "vn/sensors/compositedata.h" +#include "vn/math/position.h" + +using namespace std; +using namespace boost::python; +using namespace vn::math; +using namespace vn::sensors; +using namespace vn::protocol::uart; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(send_overloads, send, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeSettings_overloads, writeSettings, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(restoreFactorySettings_overloads, restoreFactorySettings, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(reset_overloads, reset, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeSerialBaudRate_overloads2, writeSerialBaudRate, 2, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeAsyncDataOutputType_overloads2, writeAsyncDataOutputType, 2, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeAsyncDataOutputFrequency_overloads2, writeAsyncDataOutputFrequency, 2, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeInsBasicConfigurationVn200_overloads, writeInsBasicConfigurationVn200, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeInsBasicConfigurationVn300_overloads, writeInsBasicConfigurationVn300, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeInsBasicConfigurationVn200NoStruct_overloads, writeInsBasicConfigurationVn200, 2, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeInsBasicConfigurationVn300NoStruct_overloads, writeInsBasicConfigurationVn300, 3, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeUserTag_overloads, writeUserTag, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeSerialBaudRate_overloads, writeSerialBaudRate, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeAsyncDataOutputType_overloads, writeAsyncDataOutputType, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeAsyncDataOutputFrequency_overloads, writeAsyncDataOutputFrequency, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeMagneticAndGravityReferenceVectors_overloads, writeMagneticAndGravityReferenceVectors, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeMagneticAndGravityReferenceVectorsNoStruct_overloads, writeMagneticAndGravityReferenceVectors, 2, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeFilterMeasurementsVarianceParameters_overloads, writeFilterMeasurementsVarianceParameters, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeFilterMeasurementsVarianceParametersNoStruct_overloads, writeFilterMeasurementsVarianceParameters, 4, 5) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeMagnetometerCompensation_overloads, writeMagnetometerCompensation, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeMagnetometerCompensationNoStruct_overloads, writeMagnetometerCompensation, 2, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeFilterActiveTuningParameters_overloads, writeFilterActiveTuningParameters, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeFilterActiveTuningParametersNoStruct_overloads, writeFilterActiveTuningParameters, 4, 5) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeAccelerationCompensation_overloads, writeAccelerationCompensation, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeAccelerationCompensationNoStruct_overloads, writeAccelerationCompensation, 2, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeReferenceFrameRotation_overloads, writeReferenceFrameRotation, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeCommunicationProtocolControl_overloads, writeCommunicationProtocolControl, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeCommunicationProtocolControlNoStruct_overloads, writeCommunicationProtocolControl, 7, 8) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeSynchronizationControl_overloads, writeSynchronizationControl, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeSynchronizationControlNoStruct_overloads, writeSynchronizationControl, 7, 8) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeSynchronizationStatus_overloads, writeSynchronizationStatus, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeSynchronizationStatusNoStruct_overloads, writeSynchronizationStatus, 3, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeFilterBasicControl_overloads, writeFilterBasicControl, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeFilterBasicControlNoStruct_overloads, writeFilterBasicControl, 5, 6) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeVpeBasicControl_overloads, writeVpeBasicControl, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeVpeBasicControlNoStruct_overloads, writeVpeBasicControl, 4, 5) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeVpeMagnetometerBasicTuning_overloads, writeVpeMagnetometerBasicTuning, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeVpeMagnetometerBasicTuningNoStruct_overloads, writeVpeMagnetometerBasicTuning, 3, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeVpeMagnetometerAdvancedTuning_overloads, writeVpeMagnetometerAdvancedTuning, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeVpeMagnetometerAdvancedTuningNoStruct_overloads, writeVpeMagnetometerAdvancedTuning, 5, 6) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeVpeAccelerometerBasicTuning_overloads, writeVpeAccelerometerBasicTuning, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeVpeAccelerometerBasicTuningNoStruct_overloads, writeVpeAccelerometerBasicTuning, 3, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeVpeAccelerometerAdvancedTuning_overloads, writeVpeAccelerometerAdvancedTuning, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeVpeAccelerometerAdvancedTuningNoStruct_overloads, writeVpeAccelerometerAdvancedTuning, 5, 6) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeVpeGryoBasicTuning_overloads, writeVpeGryoBasicTuning, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeVpeGryoBasicTuningNoStruct_overloads, writeVpeGryoBasicTuning, 3, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeFilterStartupGyroBias_overloads, writeFilterStartupGyroBias, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeMagnetometerCalibrationControl_overloads, writeMagnetometerCalibrationControl, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeMagnetometerCalibrationControlNoStruct_overloads, writeMagnetometerCalibrationControl, 3, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeIndoorHeadingModeControl_overloads, writeIndoorHeadingModeControl, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeVelocityCompensationMeasurement_overloads, writeVelocityCompensationMeasurement, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeVelocityCompensationControl_overloads, writeVelocityCompensationControl, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeVelocityCompensationControlNoStruct_overloads, writeVelocityCompensationControl, 3, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeGpsConfiguration_overloads, writeGpsConfiguration, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeGpsConfigurationNoStruct_overloads, writeGpsConfiguration, 2, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeGpsAntennaOffset_overloads, writeGpsAntennaOffset, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeInsAdvancedConfiguration_overloads, writeInsAdvancedConfiguration, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeInsAdvancedConfigurationNoStruct_overloads, writeInsAdvancedConfiguration, 15, 16) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeStartupFilterBiasEstimate_overloads, writeStartupFilterBiasEstimate, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeStartupFilterBiasEstimateNoStruct_overloads, writeStartupFilterBiasEstimate, 3, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeBinaryOutput1_overloads, writeBinaryOutput1, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeBinaryOutput1NoStruct_overloads, writeBinaryOutput1, 5, 6) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeBinaryOutput2_overloads, writeBinaryOutput2, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeBinaryOutput2NoStruct_overloads, writeBinaryOutput2, 5, 6) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeBinaryOutput3_overloads, writeBinaryOutput3, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeBinaryOutput3NoStruct_overloads, writeBinaryOutput3, 5, 6) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeBinaryOutput4_overloads, writeBinaryOutput4, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeBinaryOutput4NoStruct_overloads, writeBinaryOutput4, 5, 6) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeBinaryOutput5_overloads, writeBinaryOutput5, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeBinaryOutput5NoStruct_overloads, writeBinaryOutput5, 5, 6) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeDeltaThetaAndDeltaVelocityConfiguration_overloads, writeDeltaThetaAndDeltaVelocityConfiguration, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeDeltaThetaAndDeltaVelocityConfigurationNoStruct_overloads, writeDeltaThetaAndDeltaVelocityConfiguration, 3, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeReferenceVectorConfiguration_overloads, writeReferenceVectorConfiguration, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeReferenceVectorConfigurationNoStruct_overloads, writeReferenceVectorConfiguration, 5, 6) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeGyroCompensation_overloads, writeGyroCompensation, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeGyroCompensationNoStruct_overloads, writeGyroCompensation, 2, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeImuFilteringConfiguration_overloads, writeImuFilteringConfiguration, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeImuFilteringConfigurationNoStruct_overloads, writeImuFilteringConfiguration, 10, 11) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeGpsCompassBaseline_overloads, writeGpsCompassBaseline, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeGpsCompassBaselineNoStruct_overloads, writeGpsCompassBaseline, 2, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeImuRateConfiguration_overloads, writeImuRateConfiguration, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(writeImuRateConfigurationNoStruct_overloads, writeImuRateConfiguration, 4, 5) + +void sensorErrorTranslator(sensor_error const& e) +{ + PyErr_SetString(PyExc_RuntimeError, e.what()); +} + +BOOST_PYTHON_MODULE(_sensors) +{ + PyEval_InitThreads(); + + docstring_options local_docstring_options(true, true, false); + + register_exception_translator(&sensorErrorTranslator); + + class_("CompositeData") + .def("parse", static_cast(&CompositeData::parse)).staticmethod("parse") + .def_readonly("has_any_attitude", &CompositeData::hasAnyAttitude) + .def_readonly("any_attitude", &CompositeData::anyAttitude) + .def_readonly("has_yaw_pitch_roll", &CompositeData::hasYawPitchRoll) + .def_readonly("yaw_pitch_roll", &CompositeData::yawPitchRoll) + .def_readonly("has_quaternion", &CompositeData::hasQuaternion) + .def_readonly("quaternion", &CompositeData::quaternion) + .def_readonly("has_direction_cosine_matrix", &CompositeData::hasDirectionCosineMatrix) + .def_readonly("direction_cosine_matrix", &CompositeData::directionCosineMatrix) + .def_readonly("reset", &CompositeData::reset) + .def_readonly("has_any_magnetic", &CompositeData::hasAnyMagnetic) + .def_readonly("any_magnetic", &CompositeData::anyMagnetic) + .def_readonly("has_magnetic", &CompositeData::hasMagnetic) + .def_readonly("magnetic", &CompositeData::magnetic) + .def_readonly("has_magnetic_uncompensated", &CompositeData::hasMagneticUncompensated) + .def_readonly("magnetic_uncompensated", &CompositeData::magneticUncompensated) + .def_readonly("has_magnetic_ned", &CompositeData::hasMagneticNed) + .def_readonly("magnetic_ned", &CompositeData::magneticNed) + .def_readonly("has_magnetic_ecef", &CompositeData::hasMagneticEcef) + .def_readonly("magnetic_ecef", &CompositeData::magneticEcef) + .def_readonly("has_magnetic_raw", &CompositeData::hasMagneticRaw) + .def_readonly("magnetic_raw", &CompositeData::magneticRaw) + .def_readonly("has_any_acceleration", &CompositeData::hasAnyAcceleration) + .def_readonly("any_acceleration", &CompositeData::anyAcceleration) + .def_readonly("has_acceleration", &CompositeData::hasAcceleration) + .def_readonly("acceleration", &CompositeData::acceleration) + .def_readonly("has_acceleration_linear_body", &CompositeData::hasAccelerationLinearBody) + .def_readonly("acceleration_linear_body", &CompositeData::accelerationLinearBody) + .def_readonly("has_acceleration_uncompensated", &CompositeData::hasAccelerationUncompenated) + .def_readonly("acceleration_uncompensated", &CompositeData::accelerationUncompensated) + .def_readonly("has_acceleration_linear_ned", &CompositeData::hasAccelerationLinearNed) + .def_readonly("acceleration_linear_ned", &CompositeData::accelerationLinearNed) + .def_readonly("has_acceleration_linear_ecef", &CompositeData::hasAccelerationLinearEcef) + .def_readonly("acceleration_linear_ecef", &CompositeData::accelerationLinearEcef) + .def_readonly("has_acceleration_ned", &CompositeData::hasAccelerationNed) + .def_readonly("acceleration_ned", &CompositeData::accelerationNed) + .def_readonly("has_acceleration_ecef", &CompositeData::hasAccelerationEcef) + .def_readonly("acceleration_ecef", &CompositeData::accelerationEcef) + .def_readonly("has_acceleration_raw", &CompositeData::hasAccelerationRaw) + .def_readonly("acceleration_raw", &CompositeData::accelerationRaw) + .def_readonly("has_any_angular_rate", &CompositeData::hasAnyAngularRate) + .def_readonly("any_angular_rate", &CompositeData::anyAngularRate) + .def_readonly("has_angular_rate", &CompositeData::hasAngularRate) + .def_readonly("angular_rate", &CompositeData::angularRate) + .def_readonly("has_angular_rate_uncompensated", &CompositeData::hasAngularRateUncompensated) + .def_readonly("angular_rate_uncompensated", &CompositeData::angularRateUncompensated) + .def_readonly("has_angular_rate_raw", &CompositeData::hasAngularRateRaw) + .def_readonly("angular_rate_raw", &CompositeData::angularRateRaw) + .def_readonly("has_any_temperature", &CompositeData::hasAnyTemperature) + .def_readonly("any_temperature", &CompositeData::anyTemperature) + .def_readonly("has_temperature", &CompositeData::hasTemperature) + .def_readonly("temperature", &CompositeData::temperature) + .def_readonly("has_temperature_raw", &CompositeData::hasTemperatureRaw) + .def_readonly("temperature_raw", &CompositeData::temperatureRaw) + .def_readonly("has_any_pressure", &CompositeData::hasAnyPressure) + .def_readonly("any_pressure", &CompositeData::anyPressure) + .def_readonly("has_pressure", &CompositeData::hasPressure) + .def_readonly("pressure", &CompositeData::pressure) + .def_readonly("has_any_position", &CompositeData::hasAnyPosition) + .def_readonly("any_position", &CompositeData::anyPosition) + .def_readonly("has_position_gps_lla", &CompositeData::hasPositionGpsLla) + .def_readonly("position_gps_lla", &CompositeData::positionGpsLla) + .def_readonly("has_position_gps_ecef", &CompositeData::hasPositionGpsEcef) + .def_readonly("position_gps_ecef", &CompositeData::positionGpsEcef) + .def_readonly("has_position_estimated_lla", &CompositeData::hasPositionEstimatedLla) + .def_readonly("position_estimated_lla", &CompositeData::positionEstimatedLla) + .def_readonly("has_position_estimated_ecef", &CompositeData::hasPositionEstimatedEcef) + .def_readonly("position_estimated_ecef", &CompositeData::positionEstimatedEcef) + .def_readonly("has_any_velocity", &CompositeData::hasAnyVelocity) + .def_readonly("any_velocity", &CompositeData::anyVelocity) + .def_readonly("has_velocity_gps_ned", &CompositeData::hasVelocityGpsNed) + .def_readonly("velocity_gps_ned", &CompositeData::velocityGpsNed) + .def_readonly("has_velocity_gps_ecef", &CompositeData::hasVelocityGpsEcef) + .def_readonly("velocity_gps_ecef", &CompositeData::velocityGpsEcef) + .def_readonly("has_velocity_estimated_ned", &CompositeData::hasVelocityEstimatedNed) + .def_readonly("velocity_estimated_ned", &CompositeData::velocityEstimatedNed) + .def_readonly("has_velocity_estimated_ecef", &CompositeData::hasVelocityEstimatedEcef) + .def_readonly("velocity_estimated_ecef", &CompositeData::velocityEstimatedEcef) + .def_readonly("has_velocity_estimated_body", &CompositeData::hasVelocityEstimatedBody) + .def_readonly("velocity_estimated_body", &CompositeData::velocityEstimatedBody) + .def_readonly("has_delta_time", &CompositeData::hasDeltaTime) + .def_readonly("delta_time", &CompositeData::deltaTime) + .def_readonly("has_delta_theta", &CompositeData::hasDeltaTheta) + .def_readonly("delta_theta", &CompositeData::deltaTheta) + .def_readonly("has_delta_velocity", &CompositeData::hasDeltaVelocity) + .def_readonly("delta_velocity", &CompositeData::deltaVelocity) + .def_readonly("has_time_startup", &CompositeData::hasTimeStartup) + .def_readonly("time_startup", &CompositeData::timeStartup) + .def_readonly("has_time_gps", &CompositeData::hasTimeGps) + .def_readonly("time_gps", &CompositeData::timeGps) + .def_readonly("has_tow", &CompositeData::hasTow) + .def_readonly("tow", &CompositeData::tow) + .def_readonly("has_week", &CompositeData::hasWeek) + .def_readonly("week", &CompositeData::week) + .def_readonly("has_num_sats", &CompositeData::hasNumSats) + .def_readonly("num_sats", &CompositeData::numSats) + .def_readonly("has_time_sync_in", &CompositeData::hasTimeSyncIn) + .def_readonly("time_sync_in", &CompositeData::timeSyncIn) + .def_readonly("has_vpe_status", &CompositeData::hasVpeStatus) + .def_readonly("vpe_status", &CompositeData::vpeStatus) + .def_readonly("has_ins_status", &CompositeData::hasInsStatus) + .def_readonly("ins_status", &CompositeData::insStatus) + .def_readonly("has_sync_in_cnt", &CompositeData::hasSyncInCnt) + .def_readonly("sync_in_cnt", &CompositeData::syncInCnt) + .def_readonly("has_time_gps_pps", &CompositeData::hasTimeGpsPps) + .def_readonly("time_gps_pps", &CompositeData::timeGpsPps) + .def_readonly("has_gps_tow", &CompositeData::hasGpsTow) + .def_readonly("gps_tow", &CompositeData::gpsTow) + .def_readonly("has_time_utc", &CompositeData::hasTimeUtc) + .def_readonly("time_utc", &CompositeData::timeUtc) + .def_readonly("has_sens_sat", &CompositeData::hasSensSat) + .def_readonly("sens_sat", &CompositeData::sensSat) + .def_readonly("has_fix", &CompositeData::hasFix) + .def_readonly("fix", &CompositeData::fix) + .def_readonly("has_any_position_uncertainty", &CompositeData::hasAnyPositionUncertainty) + .def_readonly("any_position_uncertainty", &CompositeData::anyPositionUncertainty) + .def_readonly("has_position_uncertainty_gps_ned", &CompositeData::hasPositionUncertaintyGpsNed) + .def_readonly("position_uncertainty_gps_ned", &CompositeData::positionUncertaintyGpsNed) + .def_readonly("has_position_uncertainty_gps_ecef", &CompositeData::hasPositionUncertaintyGpsEcef) + .def_readonly("position_uncertainty_gps_ecef", &CompositeData::positionUncertaintyGpsEcef) + .def_readonly("has_position_uncertainty_estimated", &CompositeData::hasPositionUncertaintyEstimated) + .def_readonly("position_uncertainty_estimated", &CompositeData::positionUncertaintyEstimated) + .def_readonly("has_any_velocity_uncertainty", &CompositeData::hasAnyVelocityUncertainty) + .def_readonly("any_velocity_uncertainty", &CompositeData::anyVelocityUncertainty) + .def_readonly("has_velocity_uncertainty_gps", &CompositeData::hasVelocityUncertaintyGps) + .def_readonly("velocity_uncertainty_gps", &CompositeData::velocityUncertaintyGps) + .def_readonly("has_velocity_uncertainty_estimated", &CompositeData::hasVelocityUncertaintyEstimated) + .def_readonly("velocity_uncertainty_estimated", &CompositeData::velocityUncertaintyEstimated) + .def_readonly("has_time_uncertainty", &CompositeData::hasTimeUncertainty) + .def_readonly("time_uncertainty", &CompositeData::timeUncertainty) + .def_readonly("has_attitude_uncertainty", &CompositeData::hasAttitudeUncertainty) + .def_readonly("attitude_uncertainty", &CompositeData::attitudeUncertainty) + .def_readonly("has_ypr_rates", &CompositeData::hasYprRates) + .def_readonly("ypr_rates", &CompositeData::yprRates) + ; + + class_("BinaryOutputRegister", "Class representing the fields of the Binary Output registers.") + .def(init()) + .def_readwrite("async_mode", &BinaryOutputRegister::asyncMode) + .def_readwrite("rate_divisor", &BinaryOutputRegister::rateDivisor) + .def_readwrite("common_field", &BinaryOutputRegister::commonField) + .def_readwrite("time_field", &BinaryOutputRegister::timeField) + .def_readwrite("imu_field", &BinaryOutputRegister::imuField) + .def_readwrite("gps_field", &BinaryOutputRegister::gpsField) + .def_readwrite("attitude_field", &BinaryOutputRegister::attitudeField) + .def_readwrite("ins_field", &BinaryOutputRegister::insField) + ; + + class_("AsyncPacketReceivedEvent", "") + .def(self += other()) + .def("add", &VnSensor::AsyncPacketReceivedEvent::add) + .def("remove", &VnSensor::AsyncPacketReceivedEvent::remove) + .def("fire", &VnSensor::AsyncPacketReceivedEvent::fire) + ; + + // Generate wrappers for our register structures. + class_("QuaternionMagneticAccelerationAndAngularRatesRegister", "Class representing the Quaternion, Magnetic, Acceleration and Angular Rates register.") + .def(init()) + .def_readwrite("quat", &QuaternionMagneticAccelerationAndAngularRatesRegister::quat) + .def_readwrite("mag", &QuaternionMagneticAccelerationAndAngularRatesRegister::mag) + .def_readwrite("accel", &QuaternionMagneticAccelerationAndAngularRatesRegister::accel) + .def_readwrite("gyro", &QuaternionMagneticAccelerationAndAngularRatesRegister::gyro) + ; + class_("MagneticAccelerationAndAngularRatesRegister", "Class representing the Magnetic, Acceleration and Angular Rates register.") + .def(init()) + .def_readwrite("mag", &MagneticAccelerationAndAngularRatesRegister::mag) + .def_readwrite("accel", &MagneticAccelerationAndAngularRatesRegister::accel) + .def_readwrite("gyro", &MagneticAccelerationAndAngularRatesRegister::gyro) + ; + class_("MagneticAndGravityReferenceVectorsRegister", "Class representing the Magnetic and Gravity Reference Vectors register.") + .def(init()) + .def_readwrite("mag_ref", &MagneticAndGravityReferenceVectorsRegister::magRef) + .def_readwrite("acc_ref", &MagneticAndGravityReferenceVectorsRegister::accRef) + ; + class_("FilterMeasurementsVarianceParametersRegister", "Class representing the Filter Measurements Variance Parameters register.") + .def(init()) + .def_readwrite("angular_walk_variance", &FilterMeasurementsVarianceParametersRegister::angularWalkVariance) + .def_readwrite("angular_rate_variance", &FilterMeasurementsVarianceParametersRegister::angularRateVariance) + .def_readwrite("magnetic_variance", &FilterMeasurementsVarianceParametersRegister::magneticVariance) + .def_readwrite("acceleration_variance", &FilterMeasurementsVarianceParametersRegister::accelerationVariance) + ; + class_("MagnetometerCompensationRegister", "Class representing the Magnetometer Compensation register.") + .def(init()) + .def_readwrite("c", &MagnetometerCompensationRegister::c) + .def_readwrite("b", &MagnetometerCompensationRegister::b) + ; + class_("FilterActiveTuningParametersRegister", "Class representing the Filter Active Tuning Parameters register.") + .def(init()) + .def_readwrite("magnetic_disturbance_gain", &FilterActiveTuningParametersRegister::magneticDisturbanceGain) + .def_readwrite("acceleration_disturbance_gain", &FilterActiveTuningParametersRegister::accelerationDisturbanceGain) + .def_readwrite("magnetic_disturbance_memory", &FilterActiveTuningParametersRegister::magneticDisturbanceMemory) + .def_readwrite("acceleration_disturbance_memory", &FilterActiveTuningParametersRegister::accelerationDisturbanceMemory) + ; + class_("AccelerationCompensationRegister", "Class representing the Acceleration Compensation register.") + .def(init()) + .def_readwrite("c", &AccelerationCompensationRegister::c) + .def_readwrite("b", &AccelerationCompensationRegister::b) + ; + class_("YawPitchRollMagneticAccelerationAndAngularRatesRegister", "Class representing the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register.") + .def(init()) + .def_readwrite("yaw_pitch_roll", &YawPitchRollMagneticAccelerationAndAngularRatesRegister::yawPitchRoll) + .def_readwrite("mag", &YawPitchRollMagneticAccelerationAndAngularRatesRegister::mag) + .def_readwrite("accel", &YawPitchRollMagneticAccelerationAndAngularRatesRegister::accel) + .def_readwrite("gyro", &YawPitchRollMagneticAccelerationAndAngularRatesRegister::gyro) + ; + class_("CommunicationProtocolControlRegister", "Class representing the Communication Protocol Control register.") + .def(init()) + .def_readwrite("serial_count", &CommunicationProtocolControlRegister::serialCount) + .def_readwrite("serial_status", &CommunicationProtocolControlRegister::serialStatus) + .def_readwrite("spi_count", &CommunicationProtocolControlRegister::spiCount) + .def_readwrite("spi_status", &CommunicationProtocolControlRegister::spiStatus) + .def_readwrite("serial_checksum", &CommunicationProtocolControlRegister::serialChecksum) + .def_readwrite("spi_checksum", &CommunicationProtocolControlRegister::spiChecksum) + .def_readwrite("error_mode", &CommunicationProtocolControlRegister::errorMode) + ; + class_("SynchronizationControlRegister", "Class representing the Synchronization Control register.") + .def(init()) + .def_readwrite("sync_in_mode", &SynchronizationControlRegister::syncInMode) + .def_readwrite("sync_in_edge", &SynchronizationControlRegister::syncInEdge) + .def_readwrite("sync_in_skip_factor", &SynchronizationControlRegister::syncInSkipFactor) + .def_readwrite("sync_out_mode", &SynchronizationControlRegister::syncOutMode) + .def_readwrite("sync_out_polarity", &SynchronizationControlRegister::syncOutPolarity) + .def_readwrite("sync_out_skip_factor", &SynchronizationControlRegister::syncOutSkipFactor) + .def_readwrite("sync_out_pulse_width", &SynchronizationControlRegister::syncOutPulseWidth) + ; + class_("SynchronizationStatusRegister", "Class representing the Synchronization Status register.") + .def(init()) + .def_readwrite("sync_in_count", &SynchronizationStatusRegister::syncInCount) + .def_readwrite("sync_in_time", &SynchronizationStatusRegister::syncInTime) + .def_readwrite("sync_out_count", &SynchronizationStatusRegister::syncOutCount) + ; + class_("FilterBasicControlRegister", "Class representing the Filter Basic Control register.") + .def(init()) + .def_readwrite("mag_mode", &FilterBasicControlRegister::magMode) + .def_readwrite("ext_mag_mode", &FilterBasicControlRegister::extMagMode) + .def_readwrite("ext_acc_mode", &FilterBasicControlRegister::extAccMode) + .def_readwrite("ext_gyro_mode", &FilterBasicControlRegister::extGyroMode) + .def_readwrite("gyro_limit", &FilterBasicControlRegister::gyroLimit) + ; + class_("VpeBasicControlRegister", "Class representing the VPE Basic Control register.") + .def(init()) + .def_readwrite("enable", &VpeBasicControlRegister::enable) + .def_readwrite("heading_mode", &VpeBasicControlRegister::headingMode) + .def_readwrite("filtering_mode", &VpeBasicControlRegister::filteringMode) + .def_readwrite("tuning_mode", &VpeBasicControlRegister::tuningMode) + ; + class_("VpeMagnetometerBasicTuningRegister", "Class representing the VPE Magnetometer Basic Tuning register.") + .def(init()) + .def_readwrite("base_tuning", &VpeMagnetometerBasicTuningRegister::baseTuning) + .def_readwrite("adaptive_tuning", &VpeMagnetometerBasicTuningRegister::adaptiveTuning) + .def_readwrite("adaptive_filtering", &VpeMagnetometerBasicTuningRegister::adaptiveFiltering) + ; + class_("VpeMagnetometerAdvancedTuningRegister", "Class representing the VPE Magnetometer Advanced Tuning register.") + .def(init()) + .def_readwrite("min_filtering", &VpeMagnetometerAdvancedTuningRegister::minFiltering) + .def_readwrite("max_filtering", &VpeMagnetometerAdvancedTuningRegister::maxFiltering) + .def_readwrite("max_adapt_rate", &VpeMagnetometerAdvancedTuningRegister::maxAdaptRate) + .def_readwrite("disturbance_window", &VpeMagnetometerAdvancedTuningRegister::disturbanceWindow) + .def_readwrite("max_tuning", &VpeMagnetometerAdvancedTuningRegister::maxTuning) + ; + class_("VpeAccelerometerBasicTuningRegister", "Class representing the VPE Accelerometer Basic Tuning register.") + .def(init()) + .def_readwrite("base_tuning", &VpeAccelerometerBasicTuningRegister::baseTuning) + .def_readwrite("adaptive_tuning", &VpeAccelerometerBasicTuningRegister::adaptiveTuning) + .def_readwrite("adaptive_filtering", &VpeAccelerometerBasicTuningRegister::adaptiveFiltering) + ; + class_("VpeAccelerometerAdvancedTuningRegister", "Class representing the VPE Accelerometer Advanced Tuning register.") + .def(init()) + .def_readwrite("min_filtering", &VpeAccelerometerAdvancedTuningRegister::minFiltering) + .def_readwrite("max_filtering", &VpeAccelerometerAdvancedTuningRegister::maxFiltering) + .def_readwrite("max_adapt_rate", &VpeAccelerometerAdvancedTuningRegister::maxAdaptRate) + .def_readwrite("disturbance_window", &VpeAccelerometerAdvancedTuningRegister::disturbanceWindow) + .def_readwrite("max_tuning", &VpeAccelerometerAdvancedTuningRegister::maxTuning) + ; + class_("VpeGryoBasicTuningRegister", "Class representing the VPE Gryo Basic Tuning register.") + .def(init()) + .def_readwrite("angular_walk_variance", &VpeGryoBasicTuningRegister::angularWalkVariance) + .def_readwrite("base_tuning", &VpeGryoBasicTuningRegister::baseTuning) + .def_readwrite("adaptive_tuning", &VpeGryoBasicTuningRegister::adaptiveTuning) + ; + class_("MagnetometerCalibrationControlRegister", "Class representing the Magnetometer Calibration Control register.") + .def(init()) + .def_readwrite("hsi_mode", &MagnetometerCalibrationControlRegister::hsiMode) + .def_readwrite("hsi_output", &MagnetometerCalibrationControlRegister::hsiOutput) + .def_readwrite("converge_rate", &MagnetometerCalibrationControlRegister::convergeRate) + ; + class_("CalculatedMagnetometerCalibrationRegister", "Class representing the Calculated Magnetometer Calibration register.") + .def(init()) + .def_readwrite("c", &CalculatedMagnetometerCalibrationRegister::c) + .def_readwrite("b", &CalculatedMagnetometerCalibrationRegister::b) + ; + class_("VelocityCompensationControlRegister", "Class representing the Velocity Compensation Control register.") + .def(init()) + .def_readwrite("mode", &VelocityCompensationControlRegister::mode) + .def_readwrite("velocity_tuning", &VelocityCompensationControlRegister::velocityTuning) + .def_readwrite("rate_tuning", &VelocityCompensationControlRegister::rateTuning) + ; + class_("VelocityCompensationStatusRegister", "Class representing the Velocity Compensation Status register.") + .def(init()) + .def_readwrite("x", &VelocityCompensationStatusRegister::x) + .def_readwrite("x_dot", &VelocityCompensationStatusRegister::xDot) + .def_readwrite("accel_offset", &VelocityCompensationStatusRegister::accelOffset) + .def_readwrite("omega", &VelocityCompensationStatusRegister::omega) + ; + class_("ImuMeasurementsRegister", "Class representing the IMU Measurements register.") + .def(init()) + .def_readwrite("mag", &ImuMeasurementsRegister::mag) + .def_readwrite("accel", &ImuMeasurementsRegister::accel) + .def_readwrite("gyro", &ImuMeasurementsRegister::gyro) + .def_readwrite("temp", &ImuMeasurementsRegister::temp) + .def_readwrite("pressure", &ImuMeasurementsRegister::pressure) + ; + class_("GpsConfigurationRegister", "Class representing the GPS Configuration register.") + .def(init()) + .def_readwrite("mode", &GpsConfigurationRegister::mode) + .def_readwrite("pps_source", &GpsConfigurationRegister::ppsSource) + ; + class_("GpsSolutionLlaRegister", "Class representing the GPS Solution - LLA register.") + .def(init()) + .def_readwrite("time", &GpsSolutionLlaRegister::time) + .def_readwrite("week", &GpsSolutionLlaRegister::week) + .def_readwrite("gps_fix", &GpsSolutionLlaRegister::gpsFix) + .def_readwrite("num_sats", &GpsSolutionLlaRegister::numSats) + .def_readwrite("lla", &GpsSolutionLlaRegister::lla) + .def_readwrite("ned_vel", &GpsSolutionLlaRegister::nedVel) + .def_readwrite("ned_acc", &GpsSolutionLlaRegister::nedAcc) + .def_readwrite("speed_acc", &GpsSolutionLlaRegister::speedAcc) + .def_readwrite("time_acc", &GpsSolutionLlaRegister::timeAcc) + ; + class_("GpsSolutionEcefRegister", "Class representing the GPS Solution - ECEF register.") + .def(init()) + .def_readwrite("tow", &GpsSolutionEcefRegister::tow) + .def_readwrite("week", &GpsSolutionEcefRegister::week) + .def_readwrite("gps_fix", &GpsSolutionEcefRegister::gpsFix) + .def_readwrite("num_sats", &GpsSolutionEcefRegister::numSats) + .def_readwrite("position", &GpsSolutionEcefRegister::position) + .def_readwrite("velocity", &GpsSolutionEcefRegister::velocity) + .def_readwrite("pos_acc", &GpsSolutionEcefRegister::posAcc) + .def_readwrite("speed_acc", &GpsSolutionEcefRegister::speedAcc) + .def_readwrite("time_acc", &GpsSolutionEcefRegister::timeAcc) + ; + class_("InsSolutionLlaRegister", "Class representing the INS Solution - LLA register.") + .def(init()) + .def_readwrite("time", &InsSolutionLlaRegister::time) + .def_readwrite("week", &InsSolutionLlaRegister::week) + .def_readwrite("status", &InsSolutionLlaRegister::status) + .def_readwrite("yaw_pitch_roll", &InsSolutionLlaRegister::yawPitchRoll) + .def_readwrite("position", &InsSolutionLlaRegister::position) + .def_readwrite("ned_vel", &InsSolutionLlaRegister::nedVel) + .def_readwrite("att_uncertainty", &InsSolutionLlaRegister::attUncertainty) + .def_readwrite("pos_uncertainty", &InsSolutionLlaRegister::posUncertainty) + .def_readwrite("vel_uncertainty", &InsSolutionLlaRegister::velUncertainty) + ; + class_("InsSolutionEcefRegister", "Class representing the INS Solution - ECEF register.") + .def(init()) + .def_readwrite("time", &InsSolutionEcefRegister::time) + .def_readwrite("week", &InsSolutionEcefRegister::week) + .def_readwrite("status", &InsSolutionEcefRegister::status) + .def_readwrite("yaw_pitch_roll", &InsSolutionEcefRegister::yawPitchRoll) + .def_readwrite("position", &InsSolutionEcefRegister::position) + .def_readwrite("velocity", &InsSolutionEcefRegister::velocity) + .def_readwrite("att_uncertainty", &InsSolutionEcefRegister::attUncertainty) + .def_readwrite("pos_uncertainty", &InsSolutionEcefRegister::posUncertainty) + .def_readwrite("vel_uncertainty", &InsSolutionEcefRegister::velUncertainty) + ; + class_("InsBasicConfigurationRegisterVn200", "Class representing the INS Basic Configuration register for a VN-200 sensor.") + .def(init()) + .def_readwrite("scenario", &InsBasicConfigurationRegisterVn200::scenario) + .def_readwrite("ahrs_aiding", &InsBasicConfigurationRegisterVn200::ahrsAiding) + ; + class_("InsBasicConfigurationRegisterVn300", "Class representing the INS Basic Configuration register for a VN-300 sensor.") + .def(init()) + .def_readwrite("scenario", &InsBasicConfigurationRegisterVn300::scenario) + .def_readwrite("ahrs_aiding", &InsBasicConfigurationRegisterVn300::ahrsAiding) + .def_readwrite("est_baseline", &InsBasicConfigurationRegisterVn300::estBaseline) + ; + class_("InsAdvancedConfigurationRegister", "Class representing the INS Advanced Configuration register.") + .def_readwrite("use_mag", &InsAdvancedConfigurationRegister::useMag) + .def_readwrite("use_pres", &InsAdvancedConfigurationRegister::usePres) + .def_readwrite("pos_att", &InsAdvancedConfigurationRegister::posAtt) + .def_readwrite("vel_att", &InsAdvancedConfigurationRegister::velAtt) + .def_readwrite("vel_bias", &InsAdvancedConfigurationRegister::velBias) + .def_readwrite("use_foam", &InsAdvancedConfigurationRegister::useFoam) + .def_readwrite("gps_cov_type", &InsAdvancedConfigurationRegister::gpsCovType) + .def_readwrite("vel_count", &InsAdvancedConfigurationRegister::velCount) + .def_readwrite("vel_init", &InsAdvancedConfigurationRegister::velInit) + .def_readwrite("move_origin", &InsAdvancedConfigurationRegister::moveOrigin) + .def_readwrite("gps_timeout", &InsAdvancedConfigurationRegister::gpsTimeout) + .def_readwrite("delta_limit_pos", &InsAdvancedConfigurationRegister::deltaLimitPos) + .def_readwrite("delta_limit_vel", &InsAdvancedConfigurationRegister::deltaLimitVel) + .def_readwrite("min_pos_uncertainty", &InsAdvancedConfigurationRegister::minPosUncertainty) + .def_readwrite("min_vel_uncertainty", &InsAdvancedConfigurationRegister::minVelUncertainty) + ; + class_("InsStateLlaRegister", "Class representing the INS State - LLA register.") + .def(init()) + .def_readwrite("yaw_pitch_roll", &InsStateLlaRegister::yawPitchRoll) + .def_readwrite("position", &InsStateLlaRegister::position) + .def_readwrite("velocity", &InsStateLlaRegister::velocity) + .def_readwrite("accel", &InsStateLlaRegister::accel) + .def_readwrite("angular_rate", &InsStateLlaRegister::angularRate) + ; + class_("InsStateEcefRegister", "Class representing the INS State - ECEF register.") + .def(init()) + .def_readwrite("yaw_pitch_roll", &InsStateEcefRegister::yawPitchRoll) + .def_readwrite("position", &InsStateEcefRegister::position) + .def_readwrite("velocity", &InsStateEcefRegister::velocity) + .def_readwrite("accel", &InsStateEcefRegister::accel) + .def_readwrite("angular_rate", &InsStateEcefRegister::angularRate) + ; + class_("StartupFilterBiasEstimateRegister", "Class representing the Startup Filter Bias Estimate register.") + .def(init()) + .def_readwrite("gyro_bias", &StartupFilterBiasEstimateRegister::gyroBias) + .def_readwrite("accel_bias", &StartupFilterBiasEstimateRegister::accelBias) + .def_readwrite("pressure_bias", &StartupFilterBiasEstimateRegister::pressureBias) + ; + class_("DeltaThetaAndDeltaVelocityRegister", "Class representing the Delta Theta and Delta Velocity register.") + .def(init()) + .def_readwrite("delta_time", &DeltaThetaAndDeltaVelocityRegister::deltaTime) + .def_readwrite("delta_theta", &DeltaThetaAndDeltaVelocityRegister::deltaTheta) + .def_readwrite("delta_velocity", &DeltaThetaAndDeltaVelocityRegister::deltaVelocity) + ; + class_("DeltaThetaAndDeltaVelocityConfigurationRegister", "Class representing the Delta Theta and Delta Velocity Configuration register.") + .def(init()) + .def_readwrite("integration_frame", &DeltaThetaAndDeltaVelocityConfigurationRegister::integrationFrame) + .def_readwrite("gyro_compensation", &DeltaThetaAndDeltaVelocityConfigurationRegister::gyroCompensation) + .def_readwrite("accel_compensation", &DeltaThetaAndDeltaVelocityConfigurationRegister::accelCompensation) + ; + class_("ReferenceVectorConfigurationRegister", "Class representing the Reference Vector Configuration register.") + .def(init()) + .def_readwrite("use_mag_model", &ReferenceVectorConfigurationRegister::useMagModel) + .def_readwrite("use_gravity_model", &ReferenceVectorConfigurationRegister::useGravityModel) + .def_readwrite("recalc_threshold", &ReferenceVectorConfigurationRegister::recalcThreshold) + .def_readwrite("year", &ReferenceVectorConfigurationRegister::year) + .def_readwrite("position", &ReferenceVectorConfigurationRegister::position) + ; + class_("GyroCompensationRegister", "Class representing the Gyro Compensation register.") + .def(init()) + .def_readwrite("c", &GyroCompensationRegister::c) + .def_readwrite("b", &GyroCompensationRegister::b) + ; + class_("ImuFilteringConfigurationRegister", "Class representing the IMU Filtering Configuration register.") + .def(init()) + .def_readwrite("mag_window_size", &ImuFilteringConfigurationRegister::magWindowSize) + .def_readwrite("accel_window_size", &ImuFilteringConfigurationRegister::accelWindowSize) + .def_readwrite("gyro_window_size", &ImuFilteringConfigurationRegister::gyroWindowSize) + .def_readwrite("temp_window_size", &ImuFilteringConfigurationRegister::tempWindowSize) + .def_readwrite("pres_window_size", &ImuFilteringConfigurationRegister::presWindowSize) + .def_readwrite("mag_filter_mode", &ImuFilteringConfigurationRegister::magFilterMode) + .def_readwrite("accel_filter_mode", &ImuFilteringConfigurationRegister::accelFilterMode) + .def_readwrite("gyro_filter_mode", &ImuFilteringConfigurationRegister::gyroFilterMode) + .def_readwrite("temp_filter_mode", &ImuFilteringConfigurationRegister::tempFilterMode) + .def_readwrite("pres_filter_mode", &ImuFilteringConfigurationRegister::presFilterMode) + ; + class_("GpsCompassBaselineRegister", "Class representing the GPS Compass Baseline register.") + .def(init()) + .def_readwrite("position", &GpsCompassBaselineRegister::position) + .def_readwrite("uncertainty", &GpsCompassBaselineRegister::uncertainty) + ; + class_("GpsCompassEstimatedBaselineRegister", "Class representing the GPS Compass Estimated Baseline register.") + .def(init()) + .def_readwrite("est_baseline_used", &GpsCompassEstimatedBaselineRegister::estBaselineUsed) + .def_readwrite("num_meas", &GpsCompassEstimatedBaselineRegister::numMeas) + .def_readwrite("position", &GpsCompassEstimatedBaselineRegister::position) + .def_readwrite("uncertainty", &GpsCompassEstimatedBaselineRegister::uncertainty) + ; + class_("ImuRateConfigurationRegister", "Class representing the IMU Rate Configuration register.") + .def(init()) + .def_readwrite("imu_rate", &ImuRateConfigurationRegister::imuRate) + .def_readwrite("nav_divisor", &ImuRateConfigurationRegister::navDivisor) + .def_readwrite("filter_target_rate", &ImuRateConfigurationRegister::filterTargetRate) + .def_readwrite("filter_min_rate", &ImuRateConfigurationRegister::filterMinRate) + ; + class_("YawPitchRollTrueBodyAccelerationAndAngularRatesRegister", "Class representing the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register.") + .def(init()) + .def_readwrite("yaw_pitch_roll", &YawPitchRollTrueBodyAccelerationAndAngularRatesRegister::yawPitchRoll) + .def_readwrite("body_accel", &YawPitchRollTrueBodyAccelerationAndAngularRatesRegister::bodyAccel) + .def_readwrite("gyro", &YawPitchRollTrueBodyAccelerationAndAngularRatesRegister::gyro) + ; + class_("YawPitchRollTrueInertialAccelerationAndAngularRatesRegister", "Class representing the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register.") + .def(init()) + .def_readwrite("yaw_pitch_roll", &YawPitchRollTrueInertialAccelerationAndAngularRatesRegister::yawPitchRoll) + .def_readwrite("inertial_accel", &YawPitchRollTrueInertialAccelerationAndAngularRatesRegister::inertialAccel) + .def_readwrite("gyro", &YawPitchRollTrueInertialAccelerationAndAngularRatesRegister::gyro) + ; + + class_("VnSensor", "Base class for VectorNav sensors.") + .def("connect", + static_cast(&VnSensor::connect), + args("port_name", "baud_rate"), + "Connects to a VectorNav sensor with the provided connection parameters.") + //.def("connect", + // static_cast(&VnSensor::connect)) + #if !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 + .def("halt", &VnSensor::stopRequest) + .def("shutdown", &VnSensor::shutdownRequest) + .def("isStopped", &VnSensor::threadStopped) + .def("resume", &VnSensor::goRequest) + .def("unregister", &VnSensor::unregisterListners) + #endif + .def("disconnect", + &VnSensor::disconnect, + "Disconnects from the sensor.") + .def("baudrate", + &VnSensor::baudrate, + "Returns the connected baudrate of the serial port.") + //.def("changeBaudrate", &VnSensor::changeBaudrate, "Changes the serial ports connected baudrate.") + .add_property("response_timeout_ms", &VnSensor::responseTimeoutMs, &VnSensor::setResponseTimeoutMs) + .def("send", + &VnSensor::send, + send_overloads(args("toSend", "waitForReply", "errorDetectionMode"), + "Writes a raw data string to the sensor, normally appending an appropriate error detection checksum.")) + .def("write_settings", + &VnSensor::writeSettings, + writeSettings_overloads(args("waitForReply"), + "Issues a Write Settings command to the VectorNav Sensor.")) + .def("restore_factory_settings", + &VnSensor::restoreFactorySettings, + restoreFactorySettings_overloads(args("waitForReply"), + "Issues a Restore Factory Settings command to the VectorNav sensor.")) + .def("reset", + &VnSensor::reset, + reset_overloads(args("waitForReply"), + "Issues a Reset command to the VectorNav sensor.")) + .def("transaction", + &VnSensor::transaction, + "Sends a command to the sensor, computing and attaching checksum and line termination charaters as needed, and returns the response.") + .def("supported_baudrates", + &VnSensor::supportedBaudrates, + "The list of baudrates supported by VectorNav sensors.") + .def("tare", + &VnSensor::tare, + "Sends a command to tare the sensor") + .def("set_gyro_bias", + &VnSensor::setGyroBias, + "Sends a command to set the gyro's bias") + .def("magnetic_disturbance_present", + &VnSensor::magneticDisturbancePresent, + "Sends a command to inform the sensor that a magnetic disturbance is present.") + .def("acceleration_disturbance_present", + &VnSensor::accelerationDisturbancePresent, + "Sends a command to inform the sensor that an acceleration disturbance is present.") + + #if PL150 + .def_readonly("event_async_packet_received", &VnSensor::eventAsyncPacketReceived) + #else + // TODO: PL-150 would like to remove this low-level callback mechanism with a more + // implemented Event structure. +// .def("register_async_packet_received_handler", +// static_cast(&VnSensor::registerAsyncPacketReceivedHandler)) + // .def("registerAsyncPacketReceivedHandler", registerAsyncPacketReceivedHandlerPyObject) + // .def("register_raw_data_received_handler", registerRawDataReceivedHandlerPyObject) + //.def("register_async_packet_received_handler", + // &vnSensorRegisterAsyncPacketReceivedHandler) + #endif + + .def("read_serial_baud_rate", + static_cast(&VnSensor::readSerialBaudRate), + "Reads the Serial Baud Rate register.") + .def("read_serial_baud_rate", + static_cast(&VnSensor::readSerialBaudRate), + "Reads the Serial Baud Rate register for the specified port ID.") + .def("write_serial_baud_rate", + static_cast(&VnSensor::writeSerialBaudRate), + writeSerialBaudRate_overloads(args("sensor", "baudrate", "waitForReply"), + "Writes the Serial Baud Rate register.")) + .def("write_serial_baud_rate", + static_cast(&VnSensor::writeSerialBaudRate), + writeSerialBaudRate_overloads2(args("sensor", "baudrate", "waitForReply"), + "Writes the Serial Baud Rate register for the specified port ID.")) + .def("read_async_data_output_type", + static_cast(&VnSensor::readAsyncDataOutputType), + "Reads the Async Data Output Type register.") + .def("read_async_data_output_type", + static_cast(&VnSensor::readAsyncDataOutputType), + "Reads the Async Data Output Type register for the specified port ID.") + .def("write_async_data_output_type", + static_cast(&VnSensor::writeAsyncDataOutputType), + writeAsyncDataOutputType_overloads(args("sensor", "ador", "waitForReply"), + "Writes the Async Data Output Type register.")) + .def("write_async_data_output_type", + static_cast(&VnSensor::writeAsyncDataOutputType), + writeAsyncDataOutputType_overloads2(args("sensor", "ador", "waitForReply"), + "Writes the Async Data Output Type register.")) + .def("read_async_data_output_frequency", + static_cast(&VnSensor::readAsyncDataOutputFrequency), + "Reads the Async Data Output Frequency register.") + .def("read_async_data_output_frequency", + static_cast(&VnSensor::readAsyncDataOutputFrequency), + "Reads the Async Data Output Frequency register for the specified port ID.") + .def("write_async_data_output_frequency", + static_cast(&VnSensor::writeAsyncDataOutputFrequency), + writeAsyncDataOutputFrequency_overloads(args("sensor", "adof", "waitForReply"), + "Writes the Async Data Output Frequency register.")) + .def("write_async_data_output_frequency", + static_cast(&VnSensor::writeAsyncDataOutputFrequency), + writeAsyncDataOutputFrequency_overloads2(args("sensor", "adof", "waitForReply"), + "Writes the Async Data Output Frequency register for the specified port ID.")) + .def("write_binary_output_1", + &VnSensor::writeBinaryOutput1, + writeBinaryOutput1_overloads("Writes the Binary Output 1 register.")) + .def("write_binary_output_2", + &VnSensor::writeBinaryOutput2, + writeBinaryOutput2_overloads("Writes the Binary Output 2 register.")) + .def("write_binary_output_3", + &VnSensor::writeBinaryOutput3, + writeBinaryOutput3_overloads("Writes the Binary Output 3 register.")) + .def("read_binary_output_1", + &VnSensor::readBinaryOutput1, + "Reads the Binary Output 1 register.") + .def("read_binary_output_2", + &VnSensor::readBinaryOutput2, + "Reads the Binary Output 2 register.") + .def("read_binary_output_3", + &VnSensor::readBinaryOutput3, + "Reads the Binary Output 3 register.") + .def("write_ins_basic_configuration_vn200", + static_cast(&VnSensor::writeInsBasicConfigurationVn200), + writeInsBasicConfigurationVn200_overloads(args("sensor", "fields", "waitForReply"), + "Writes the INS Basic Configuration register for a VN-200 sensor.")) + .def("write_ins_basic_configuration_vn200", + static_cast(&VnSensor::writeInsBasicConfigurationVn200), + writeInsBasicConfigurationVn200NoStruct_overloads()) + .def("write_ins_basic_configuration_vn300", + static_cast(&VnSensor::writeInsBasicConfigurationVn300), + writeInsBasicConfigurationVn300_overloads(args("sensor", "fields", "waitForReply"), + "Writes the INS Basic Configuration register for a VN-300 sensor.")) + .def("write_ins_basic_configuration_vn300", + static_cast(&VnSensor::writeInsBasicConfigurationVn300), + writeInsBasicConfigurationVn300NoStruct_overloads()) + .def("read_user_tag", &VnSensor::readUserTag, "Reads the User Tag register.") + .def("write_user_tag", &VnSensor::writeUserTag, writeUserTag_overloads(args("sensor", "tag", "waitForReply"), "Writes the User Tag register.")) + .def("read_model_number", &VnSensor::readModelNumber, "Reads the Model Number register.") + .def("read_hardware_revision", &VnSensor::readHardwareRevision, "Reads the Hardware Revision register.") + .def("read_serial_number", &VnSensor::readSerialNumber, "Reads the Serial Number register.") + .def("read_firmware_version", &VnSensor::readFirmwareVersion, "Reads the Firmware Version register.") + .def("read_yaw_pitch_roll", &VnSensor::readYawPitchRoll, "Reads the Yaw Pitch Roll register.") + .def("read_attitude_quaternion", &VnSensor::readAttitudeQuaternion, "Reads the Attitude Quaternion register.") + .def("read_quaternion_magnetic_acceleration_and_angular_rates", &VnSensor::readQuaternionMagneticAccelerationAndAngularRates, "Reads the Quaternion, Magnetic, Acceleration and Angular Rates register.") + .def("read_magnetic_measurements", &VnSensor::readMagneticMeasurements, "Reads the Magnetic Measurements register.") + .def("read_acceleration_measurements", &VnSensor::readAccelerationMeasurements, "Reads the Acceleration Measurements register.") + .def("read_angular_rate_measurements", &VnSensor::readAngularRateMeasurements, "Reads the Angular Rate Measurements register.") + .def("read_magnetic_acceleration_and_angular_rates", &VnSensor::readMagneticAccelerationAndAngularRates, "Reads the Magnetic, Acceleration and Angular Rates register.") + .def("read_magnetic_and_gravity_reference_vectors", &VnSensor::readMagneticAndGravityReferenceVectors, "Reads the Magnetic and Gravity Reference Vectors register.") + .def("write_magnetic_and_gravity_reference_vectors", + static_cast(&VnSensor::writeMagneticAndGravityReferenceVectors), + writeMagneticAndGravityReferenceVectors_overloads(args("sensor", "fields", "waitForReply"), "Write the Magnetic and Gravity Reference Vectors register.")) + .def("write_magnetic_and_gravity_reference_vectors", + static_cast(&VnSensor::writeMagneticAndGravityReferenceVectors), + writeMagneticAndGravityReferenceVectorsNoStruct_overloads(args("sensor", "magRef", "accRef", "waitForReply"), "Writes the Magnetic and Gravity Reference Vectors register.")) + .def("read_filter_measurements_variance_parameters", &VnSensor::readFilterMeasurementsVarianceParameters, "Reads the Filter Measurements Variance Parameters register.") + .def("write_filter_measurements_variance_parameters", + static_cast(&VnSensor::writeFilterMeasurementsVarianceParameters), + writeFilterMeasurementsVarianceParameters_overloads(args("sensor", "fields", "waitForReply"), "Write the Filter Measurements Variance Parameters register.")) + .def("write_filter_measurements_variance_parameters", + static_cast(&VnSensor::writeFilterMeasurementsVarianceParameters), + writeFilterMeasurementsVarianceParametersNoStruct_overloads(args("sensor", "angularWalkVariance", "angularRateVariance", "magneticVariance", "accelerationVariance", "waitForReply"), "Writes the Filter Measurements Variance Parameters register.")) + .def("read_magnetometer_compensation", &VnSensor::readMagnetometerCompensation, "Reads the Magnetometer Compensation register.") + .def("write_magnetometer_compensation", + static_cast(&VnSensor::writeMagnetometerCompensation), + writeMagnetometerCompensation_overloads(args("sensor", "fields", "waitForReply"), "Write the Magnetometer Compensation register.")) + .def("write_magnetometer_compensation", + static_cast(&VnSensor::writeMagnetometerCompensation), + writeMagnetometerCompensationNoStruct_overloads(args("sensor", "c", "b", "waitForReply"), "Writes the Magnetometer Compensation register.")) + .def("read_filter_active_tuning_parameters", &VnSensor::readFilterActiveTuningParameters, "Reads the Filter Active Tuning Parameters register.") + .def("write_filter_active_tuning_parameters", + static_cast(&VnSensor::writeFilterActiveTuningParameters), + writeFilterActiveTuningParameters_overloads(args("sensor", "fields", "waitForReply"), "Write the Filter Active Tuning Parameters register.")) + .def("write_filter_active_tuning_parameters", + static_cast(&VnSensor::writeFilterActiveTuningParameters), + writeFilterActiveTuningParametersNoStruct_overloads(args("sensor", "magneticDisturbanceGain", "accelerationDisturbanceGain", "magneticDisturbanceMemory", "accelerationDisturbanceMemory", "waitForReply"), "Writes the Filter Active Tuning Parameters register.")) + .def("read_acceleration_compensation", &VnSensor::readAccelerationCompensation, "Reads the Acceleration Compensation register.") + .def("write_acceleration_compensation", + static_cast(&VnSensor::writeAccelerationCompensation), + writeAccelerationCompensation_overloads(args("sensor", "fields", "waitForReply"), "Write the Acceleration Compensation register.")) + .def("write_acceleration_compensation", + static_cast(&VnSensor::writeAccelerationCompensation), + writeAccelerationCompensationNoStruct_overloads(args("sensor", "c", "b", "waitForReply"), "Writes the Acceleration Compensation register.")) + .def("read_reference_frame_rotation", &VnSensor::readReferenceFrameRotation, "Reads the Reference Frame Rotation register.") + .def("write_reference_frame_rotation", &VnSensor::writeReferenceFrameRotation, writeReferenceFrameRotation_overloads(args("sensor", "c", "waitForReply"), "Writes the Reference Frame Rotation register.")) + .def("read_yaw_pitch_roll_magnetic_acceleration_and_angular_rates", &VnSensor::readYawPitchRollMagneticAccelerationAndAngularRates, "Reads the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register.") + .def("read_communication_protocol_control", &VnSensor::readCommunicationProtocolControl, "Reads the Communication Protocol Control register.") + .def("write_communication_protocol_control", + static_cast(&VnSensor::writeCommunicationProtocolControl), + writeCommunicationProtocolControl_overloads(args("sensor", "fields", "waitForReply"), "Write the Communication Protocol Control register.")) + .def("write_communication_protocol_control", + static_cast(&VnSensor::writeCommunicationProtocolControl), + writeCommunicationProtocolControlNoStruct_overloads(args("sensor", "serialCount", "serialStatus", "spiCount", "spiStatus", "serialChecksum", "spiChecksum", "errorMode", "waitForReply"), "Writes the Communication Protocol Control register.")) + .def("read_synchronization_control", &VnSensor::readSynchronizationControl, "Reads the Synchronization Control register.") + .def("write_synchronization_control", + static_cast(&VnSensor::writeSynchronizationControl), + writeSynchronizationControl_overloads(args("sensor", "fields", "waitForReply"), "Write the Synchronization Control register.")) + .def("write_synchronization_control", + static_cast(&VnSensor::writeSynchronizationControl), + writeSynchronizationControlNoStruct_overloads(args("sensor", "syncInMode", "syncInEdge", "syncInSkipFactor", "syncOutMode", "syncOutPolarity", "syncOutSkipFactor", "syncOutPulseWidth", "waitForReply"), "Writes the Synchronization Control register.")) + .def("read_synchronization_status", &VnSensor::readSynchronizationStatus, "Reads the Synchronization Status register.") + .def("write_synchronization_status", + static_cast(&VnSensor::writeSynchronizationStatus), + writeSynchronizationStatus_overloads(args("sensor", "fields", "waitForReply"), "Write the Synchronization Status register.")) + .def("write_synchronization_status", + static_cast(&VnSensor::writeSynchronizationStatus), + writeSynchronizationStatusNoStruct_overloads(args("sensor", "syncInCount", "syncInTime", "syncOutCount", "waitForReply"), "Writes the Synchronization Status register.")) + .def("read_filter_basic_control", &VnSensor::readFilterBasicControl, "Reads the Filter Basic Control register.") + .def("write_filter_basic_control", + static_cast(&VnSensor::writeFilterBasicControl), + writeFilterBasicControl_overloads(args("sensor", "fields", "waitForReply"), "Write the Filter Basic Control register.")) + .def("write_filter_basic_control", + static_cast(&VnSensor::writeFilterBasicControl), + writeFilterBasicControlNoStruct_overloads(args("sensor", "magMode", "extMagMode", "extAccMode", "extGyroMode", "gyroLimit", "waitForReply"), "Writes the Filter Basic Control register.")) + .def("read_vpe_basic_control", &VnSensor::readVpeBasicControl, "Reads the VPE Basic Control register.") + .def("write_vpe_basic_control", + static_cast(&VnSensor::writeVpeBasicControl), + writeVpeBasicControl_overloads(args("sensor", "fields", "waitForReply"), "Write the VPE Basic Control register.")) + .def("write_vpe_basic_control", + static_cast(&VnSensor::writeVpeBasicControl), + writeVpeBasicControlNoStruct_overloads(args("sensor", "enable", "headingMode", "filteringMode", "tuningMode", "waitForReply"), "Writes the VPE Basic Control register.")) + .def("read_vpe_magnetometer_basic_tuning", &VnSensor::readVpeMagnetometerBasicTuning, "Reads the VPE Magnetometer Basic Tuning register.") + .def("write_vpe_magnetometer_basic_tuning", + static_cast(&VnSensor::writeVpeMagnetometerBasicTuning), + writeVpeMagnetometerBasicTuning_overloads(args("sensor", "fields", "waitForReply"), "Write the VPE Magnetometer Basic Tuning register.")) + .def("write_vpe_magnetometer_basic_tuning", + static_cast(&VnSensor::writeVpeMagnetometerBasicTuning), + writeVpeMagnetometerBasicTuningNoStruct_overloads(args("sensor", "baseTuning", "adaptiveTuning", "adaptiveFiltering", "waitForReply"), "Writes the VPE Magnetometer Basic Tuning register.")) + .def("read_vpe_magnetometer_advanced_tuning", &VnSensor::readVpeMagnetometerAdvancedTuning, "Reads the VPE Magnetometer Advanced Tuning register.") + .def("write_vpe_magnetometer_advanced_tuning", + static_cast(&VnSensor::writeVpeMagnetometerAdvancedTuning), + writeVpeMagnetometerAdvancedTuning_overloads(args("sensor", "fields", "waitForReply"), "Write the VPE Magnetometer Advanced Tuning register.")) + .def("write_vpe_magnetometer_advanced_tuning", + static_cast(&VnSensor::writeVpeMagnetometerAdvancedTuning), + writeVpeMagnetometerAdvancedTuningNoStruct_overloads(args("sensor", "minFiltering", "maxFiltering", "maxAdaptRate", "disturbanceWindow", "maxTuning", "waitForReply"), "Writes the VPE Magnetometer Advanced Tuning register.")) + .def("read_vpe_accelerometer_basic_tuning", &VnSensor::readVpeAccelerometerBasicTuning, "Reads the VPE Accelerometer Basic Tuning register.") + .def("write_vpe_accelerometer_basic_tuning", + static_cast(&VnSensor::writeVpeAccelerometerBasicTuning), + writeVpeAccelerometerBasicTuning_overloads(args("sensor", "fields", "waitForReply"), "Write the VPE Accelerometer Basic Tuning register.")) + .def("write_vpe_accelerometer_basic_tuning", + static_cast(&VnSensor::writeVpeAccelerometerBasicTuning), + writeVpeAccelerometerBasicTuningNoStruct_overloads(args("sensor", "baseTuning", "adaptiveTuning", "adaptiveFiltering", "waitForReply"), "Writes the VPE Accelerometer Basic Tuning register.")) + .def("read_vpe_accelerometer_advanced_tuning", &VnSensor::readVpeAccelerometerAdvancedTuning, "Reads the VPE Accelerometer Advanced Tuning register.") + .def("write_vpe_accelerometer_advanced_tuning", + static_cast(&VnSensor::writeVpeAccelerometerAdvancedTuning), + writeVpeAccelerometerAdvancedTuning_overloads(args("sensor", "fields", "waitForReply"), "Write the VPE Accelerometer Advanced Tuning register.")) + .def("write_vpe_accelerometer_advanced_tuning", + static_cast(&VnSensor::writeVpeAccelerometerAdvancedTuning), + writeVpeAccelerometerAdvancedTuningNoStruct_overloads(args("sensor", "minFiltering", "maxFiltering", "maxAdaptRate", "disturbanceWindow", "maxTuning", "waitForReply"), "Writes the VPE Accelerometer Advanced Tuning register.")) + .def("read_vpe_gryo_basic_tuning", &VnSensor::readVpeGryoBasicTuning, "Reads the VPE Gryo Basic Tuning register.") + .def("write_vpe_gryo_basic_tuning", + static_cast(&VnSensor::writeVpeGryoBasicTuning), + writeVpeGryoBasicTuning_overloads(args("sensor", "fields", "waitForReply"), "Write the VPE Gryo Basic Tuning register.")) + .def("write_vpe_gryo_basic_tuning", + static_cast(&VnSensor::writeVpeGryoBasicTuning), + writeVpeGryoBasicTuningNoStruct_overloads(args("sensor", "angularWalkVariance", "baseTuning", "adaptiveTuning", "waitForReply"), "Writes the VPE Gryo Basic Tuning register.")) + .def("read_filter_startup_gyro_bias", &VnSensor::readFilterStartupGyroBias, "Reads the Filter Startup Gyro Bias register.") + .def("write_filter_startup_gyro_bias", &VnSensor::writeFilterStartupGyroBias, writeFilterStartupGyroBias_overloads(args("sensor", "bias", "waitForReply"), "Writes the Filter Startup Gyro Bias register.")) + .def("read_magnetometer_calibration_control", &VnSensor::readMagnetometerCalibrationControl, "Reads the Magnetometer Calibration Control register.") + .def("write_magnetometer_calibration_control", + static_cast(&VnSensor::writeMagnetometerCalibrationControl), + writeMagnetometerCalibrationControl_overloads(args("sensor", "fields", "waitForReply"), "Write the Magnetometer Calibration Control register.")) + .def("write_magnetometer_calibration_control", + static_cast(&VnSensor::writeMagnetometerCalibrationControl), + writeMagnetometerCalibrationControlNoStruct_overloads(args("sensor", "hsiMode", "hsiOutput", "convergeRate", "waitForReply"), "Writes the Magnetometer Calibration Control register.")) + .def("read_calculated_magnetometer_calibration", &VnSensor::readCalculatedMagnetometerCalibration, "Reads the Calculated Magnetometer Calibration register.") + .def("read_indoor_heading_mode_control", &VnSensor::readIndoorHeadingModeControl, "Reads the Indoor Heading Mode Control register.") + .def("write_indoor_heading_mode_control", &VnSensor::writeIndoorHeadingModeControl, writeIndoorHeadingModeControl_overloads(args("sensor", "maxRateError", "waitForReply"), "Writes the Indoor Heading Mode Control register.")) + .def("read_velocity_compensation_measurement", &VnSensor::readVelocityCompensationMeasurement, "Reads the Velocity Compensation Measurement register.") + .def("write_velocity_compensation_measurement", &VnSensor::writeVelocityCompensationMeasurement, writeVelocityCompensationMeasurement_overloads(args("sensor", "velocity", "waitForReply"), "Writes the Velocity Compensation Measurement register.")) + .def("read_velocity_compensation_control", &VnSensor::readVelocityCompensationControl, "Reads the Velocity Compensation Control register.") + .def("write_velocity_compensation_control", + static_cast(&VnSensor::writeVelocityCompensationControl), + writeVelocityCompensationControl_overloads(args("sensor", "fields", "waitForReply"), "Write the Velocity Compensation Control register.")) + .def("write_velocity_compensation_control", + static_cast(&VnSensor::writeVelocityCompensationControl), + writeVelocityCompensationControlNoStruct_overloads(args("sensor", "mode", "velocityTuning", "rateTuning", "waitForReply"), "Writes the Velocity Compensation Control register.")) + .def("read_velocity_compensation_status", &VnSensor::readVelocityCompensationStatus, "Reads the Velocity Compensation Status register.") + .def("read_imu_measurements", &VnSensor::readImuMeasurements, "Reads the IMU Measurements register.") + .def("read_gps_configuration", &VnSensor::readGpsConfiguration, "Reads the GPS Configuration register.") + .def("write_gps_configuration", + static_cast(&VnSensor::writeGpsConfiguration), + writeGpsConfiguration_overloads(args("sensor", "fields", "waitForReply"), "Write the GPS Configuration register.")) + .def("write_gps_configuration", + static_cast(&VnSensor::writeGpsConfiguration), + writeGpsConfigurationNoStruct_overloads(args("sensor", "mode", "ppsSource", "waitForReply"), "Writes the GPS Configuration register.")) + .def("read_gps_antenna_offset", &VnSensor::readGpsAntennaOffset, "Reads the GPS Antenna Offset register.") + .def("write_gps_antenna_offset", &VnSensor::writeGpsAntennaOffset, writeGpsAntennaOffset_overloads(args("sensor", "position", "waitForReply"), "Writes the GPS Antenna Offset register.")) + .def("read_gps_solution-lla", &VnSensor::readGpsSolutionLla, "Reads the GPS Solution - LLA register.") + .def("read_gps_solution-ecef", &VnSensor::readGpsSolutionEcef, "Reads the GPS Solution - ECEF register.") + .def("read_ins_solution-lla", &VnSensor::readInsSolutionLla, "Reads the INS Solution - LLA register.") + .def("read_ins_solution-ecef", &VnSensor::readInsSolutionEcef, "Reads the INS Solution - ECEF register.") + .def("read_ins_basic_configuration_vn200", &VnSensor::readInsBasicConfigurationVn200, "Reads the INS Basic Configuration register for a VN-200 sensor.") + .def("read_ins_basic_configuration_vn300", &VnSensor::readInsBasicConfigurationVn300, "Reads the INS Basic Configuration register for a VN-300 sensor.") + .def("read_ins_advanced_configuration", &VnSensor::readInsAdvancedConfiguration, "Reads the INS Advanced Configuration register.") + .def("write_ins_advanced_configuration", + static_cast(&VnSensor::writeInsAdvancedConfiguration), + writeInsAdvancedConfiguration_overloads(args("sensor", "fields", "waitForReply"), "Write the INS Advanced Configuration register.")) + .def("read_ins_state-lla", &VnSensor::readInsStateLla, "Reads the INS State - LLA register.") + .def("read_ins_state-ecef", &VnSensor::readInsStateEcef, "Reads the INS State - ECEF register.") + .def("read_startup_filter_bias_estimate", &VnSensor::readStartupFilterBiasEstimate, "Reads the Startup Filter Bias Estimate register.") + .def("write_startup_filter_bias_estimate", + static_cast(&VnSensor::writeStartupFilterBiasEstimate), + writeStartupFilterBiasEstimate_overloads(args("sensor", "fields", "waitForReply"), "Write the Startup Filter Bias Estimate register.")) + .def("write_startup_filter_bias_estimate", + static_cast(&VnSensor::writeStartupFilterBiasEstimate), + writeStartupFilterBiasEstimateNoStruct_overloads(args("sensor", "gyroBias", "accelBias", "pressureBias", "waitForReply"), "Writes the Startup Filter Bias Estimate register.")) + .def("read_delta_theta_and_delta_velocity", &VnSensor::readDeltaThetaAndDeltaVelocity, "Reads the Delta Theta and Delta Velocity register.") + .def("read_delta_theta_and_delta_velocity_configuration", &VnSensor::readDeltaThetaAndDeltaVelocityConfiguration, "Reads the Delta Theta and Delta Velocity Configuration register.") + .def("write_delta_theta_and_delta_velocity_configuration", + static_cast(&VnSensor::writeDeltaThetaAndDeltaVelocityConfiguration), + writeDeltaThetaAndDeltaVelocityConfiguration_overloads(args("sensor", "fields", "waitForReply"), "Write the Delta Theta and Delta Velocity Configuration register.")) + .def("write_delta_theta_and_delta_velocity_configuration", + static_cast(&VnSensor::writeDeltaThetaAndDeltaVelocityConfiguration), + writeDeltaThetaAndDeltaVelocityConfigurationNoStruct_overloads(args("sensor", "integrationFrame", "gyroCompensation", "accelCompensation", "waitForReply"), "Writes the Delta Theta and Delta Velocity Configuration register.")) + .def("read_reference_vector_configuration", &VnSensor::readReferenceVectorConfiguration, "Reads the Reference Vector Configuration register.") + .def("write_reference_vector_configuration", + static_cast(&VnSensor::writeReferenceVectorConfiguration), + writeReferenceVectorConfiguration_overloads(args("sensor", "fields", "waitForReply"), "Write the Reference Vector Configuration register.")) + .def("write_reference_vector_configuration", + static_cast(&VnSensor::writeReferenceVectorConfiguration), + writeReferenceVectorConfigurationNoStruct_overloads(args("sensor", "useMagModel", "useGravityModel", "recalcThreshold", "year", "position", "waitForReply"), "Writes the Reference Vector Configuration register.")) + .def("read_gyro_compensation", &VnSensor::readGyroCompensation, "Reads the Gyro Compensation register.") + .def("write_gyro_compensation", + static_cast(&VnSensor::writeGyroCompensation), + writeGyroCompensation_overloads(args("sensor", "fields", "waitForReply"), "Write the Gyro Compensation register.")) + .def("write_gyro_compensation", + static_cast(&VnSensor::writeGyroCompensation), + writeGyroCompensationNoStruct_overloads(args("sensor", "c", "b", "waitForReply"), "Writes the Gyro Compensation register.")) + .def("read_imu_filtering_configuration", &VnSensor::readImuFilteringConfiguration, "Reads the IMU Filtering Configuration register.") + .def("write_imu_filtering_configuration", + static_cast(&VnSensor::writeImuFilteringConfiguration), + writeImuFilteringConfiguration_overloads(args("sensor", "fields", "waitForReply"), "Write the IMU Filtering Configuration register.")) + .def("write_imu_filtering_configuration", + static_cast(&VnSensor::writeImuFilteringConfiguration), + writeImuFilteringConfigurationNoStruct_overloads(args("sensor", "magWindowSize", "accelWindowSize", "gyroWindowSize", "tempWindowSize", "presWindowSize", "magFilterMode", "accelFilterMode", "gyroFilterMode", "tempFilterMode", "presFilterMode", "waitForReply"), "Writes the IMU Filtering Configuration register.")) + .def("read_gps_compass_baseline", &VnSensor::readGpsCompassBaseline, "Reads the GPS Compass Baseline register.") + .def("write_gps_compass_baseline", + static_cast(&VnSensor::writeGpsCompassBaseline), + writeGpsCompassBaseline_overloads(args("sensor", "fields", "waitForReply"), "Write the GPS Compass Baseline register.")) + .def("write_gps_compass_baseline", + static_cast(&VnSensor::writeGpsCompassBaseline), + writeGpsCompassBaselineNoStruct_overloads(args("sensor", "position", "uncertainty", "waitForReply"), "Writes the GPS Compass Baseline register.")) + .def("read_gps_compass_estimated_baseline", &VnSensor::readGpsCompassEstimatedBaseline, "Reads the GPS Compass Estimated Baseline register.") + .def("read_imu_rate_configuration", &VnSensor::readImuRateConfiguration, "Reads the IMU Rate Configuration register.") + .def("write_imu_rate_configuration", + static_cast(&VnSensor::writeImuRateConfiguration), + writeImuRateConfiguration_overloads(args("sensor", "fields", "waitForReply"), "Write the IMU Rate Configuration register.")) + .def("write_imu_rate_configuration", + static_cast(&VnSensor::writeImuRateConfiguration), + writeImuRateConfigurationNoStruct_overloads(args("sensor", "imuRate", "navDivisor", "filterTargetRate", "filterMinRate", "waitForReply"), "Writes the IMU Rate Configuration register.")) + .def("read_yaw_pitch_roll_true_body_acceleration_and_angular_rates", &VnSensor::readYawPitchRollTrueBodyAccelerationAndAngularRates, "Reads the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register.") + .def("read_yaw_pitch_roll_true_inertial_acceleration_and_angular_rates", &VnSensor::readYawPitchRollTrueInertialAccelerationAndAngularRates, "Reads the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register.") + ; + + class_("EzAsyncData", "Provides easy and reliable access to asynchronous data from a VectorNav sensor at the cost of a slight performance hit.", no_init) + .def("connect", + &EzAsyncData::connect, + //return_value_policy(), + return_value_policy(), + args("port_name", "baud_rate"), + "Connects to a VectorNav sensor with the provided connection parameters.") + .staticmethod("connect") + .def("disconnect", &EzAsyncData::disconnect) + .def_readonly("current_data", &EzAsyncData::currentData) + + // TODO: Would like to make the ez.sensor access as a property instead of a function. + .def("sensor", &EzAsyncData::sensor, return_internal_reference<>()) + //.def_readonly + // ("sensor", + // &EzAsyncData::sensor, + // return_value_policy()) + //.def_readonly("sensor", &EzAsyncData::sensor, return_internal_reference<>()) + //.def_readonly("sensor", &EzAsyncData::sensor, return_value_policy()) + //.def_readonly("sensor", &EzAsyncData::sensor) + + .def("get_next_data", static_cast(&EzAsyncData::getNextData)) + .def("get_next_data", static_cast(&EzAsyncData::getNextData)) + ; +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/sensors/searcher.cpp b/ext/vnproglib-1.1.0.115/src/vn/sensors/searcher.cpp new file mode 100755 index 0000000..121e010 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/sensors/searcher.cpp @@ -0,0 +1,202 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/sensors/searcher.h" + +#include + +#include "vn/xplat/serialport.h" +#include "vn/xplat/event.h" +#include "vn/xplat/thread.h" +#include "vn/protocol/uart/packetfinder.h" + +using namespace std; +using namespace vn::xplat; +using namespace vn::protocol::uart; + +namespace vn { +namespace sensors { + +struct TestHelper +{ + SerialPort *serialPort; + PacketFinder *packetFinder; + Event waitForCheckingOnPort; + + TestHelper(SerialPort *serialPort, PacketFinder *packetFinder) : + serialPort(serialPort), + packetFinder(packetFinder) + { } +}; + +struct SearchHelper +{ + Event finishedSearchingPort; + bool sensorFound; + string portName; + uint32_t foundBaudrate; + + explicit SearchHelper(const string &portName) : + sensorFound(false), + portName(portName) + { } +}; + +void testDataReceivedHandler(void* userData); +void testValidPacketFoundHandler(void *userData, Packet &packet, size_t runningIndexOfPacketStart, TimeStamp timestamp); +void searchThread(void* routineData); + +// Collection of baudrates to test for sensors. They are listed in order of +// liklyness and fastness. +#if __cplusplus >= 201103L +vector TestBaudrates { 115200, 128000, 230400, 460800, 921600, 57600, 38400, 19200, 9600 }; +#else +uint32_t TestBaudratesRaw[] = { 115200, 128000, 230400, 460800, 921600, 57600, 38400, 19200, 9600 }; +#endif + +bool Searcher::search(const string &portName, uint32_t *foundBaudrate) +{ + #if __cplusplus < 201103L + vector TestBaudrates(TestBaudratesRaw, TestBaudratesRaw + sizeof(TestBaudratesRaw) / sizeof(TestBaudratesRaw[0])); + #endif + for (vector::const_iterator it = TestBaudrates.begin(); it != TestBaudrates.end(); ++it) + { + uint32_t curBaudrate = *it; + + if (test(portName, curBaudrate)) + { + *foundBaudrate = curBaudrate; + + return true; + } + } + + return false; +} + +vector > Searcher::search() +{ + return search(SerialPort::getPortNames()); +} + +vector > Searcher::search(vector portsToCheck) +{ + list helpers; + vector > result; + + // Start threads to test each port. + for (vector::const_iterator it = portsToCheck.begin(); it != portsToCheck.end(); ++it) + { + SearchHelper *sh = new SearchHelper(*it); + + helpers.push_back(sh); + + Thread::startNew(searchThread, sh); + } + + // Wait for each thread to finish. + for (list::const_iterator it = helpers.begin(); it != helpers.end(); ++it) + { + SearchHelper* sh = (*it); + + sh->finishedSearchingPort.wait(); + + if (sh->sensorFound) + { + result.push_back(pair(sh->portName, sh->foundBaudrate)); + } + + delete sh; + } + + return result; +} + +bool Searcher::test(string portName, uint32_t baudrate) +{ + SerialPort sp(portName, baudrate); + PacketFinder pf; + + try + { + sp.open(); + } + catch (invalid_argument) + { + // Probably an unsupported baudrate for the port. + return false; + } + + TestHelper th(&sp, &pf); + + pf.registerPossiblePacketFoundHandler(&th, testValidPacketFoundHandler); + sp.registerDataReceivedHandler(&th, testDataReceivedHandler); + + // Wait for a few milliseconds to see if we receive any asynchronous + // data packets. + if (th.waitForCheckingOnPort.waitMs(50) == Event::WAIT_SIGNALED) + { + sp.close(); + + return true; + } + + // We have received any asynchronous data packets so let's try sending + // some commands. + for (size_t i = 0; i < 9; i++) + { + sp.write("$VNRRG,01*XX\r\n", 14); + + if (th.waitForCheckingOnPort.waitMs(50) == Event::WAIT_SIGNALED) + { + sp.close(); + + return true; + } + } + + sp.close(); + + return false; +} + +void searchThread(void* routineData) +{ + SearchHelper *sh = static_cast(routineData); + + sh->sensorFound = Searcher::search(sh->portName, &sh->foundBaudrate); + + sh->finishedSearchingPort.signal(); +} + +void testDataReceivedHandler(void* userData) +{ + TestHelper *th = static_cast(userData); + + char buffer[0x100]; + size_t numOfBytesRead; + + th->serialPort->read(buffer, 0x100, numOfBytesRead); + + th->packetFinder->processReceivedData(buffer, numOfBytesRead); +} + +#if defined(_MSC_VER) + #pragma warning(push) + + // Disable warnings about unused parameters. + #pragma warning(disable:4100) +#endif + +void testValidPacketFoundHandler(void *userData, Packet &packet, size_t runningIndexOfPacketStart, TimeStamp timestamp) +{ + TestHelper *th = static_cast(userData); + + th->waitForCheckingOnPort.signal(); +} + +#if defined (_MSC_VER) + #pragma warning(pop) +#endif + +} +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/sensors/sensors.cpp b/ext/vnproglib-1.1.0.115/src/vn/sensors/sensors.cpp new file mode 100755 index 0000000..35eeced --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/sensors/sensors.cpp @@ -0,0 +1,3392 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/sensors/sensors.h" + +#include +#include +#include +#include + +#include "vn/xplat/serialport.h" +#include "vn/xplat/criticalsection.h" +#include "vn/xplat/time.h" +#include "vn/xplat/event.h" +#include "vn/exceptions.h" +#include "vn/data/error_detection.h" +#include "vn/math/vector.h" +#include "vn/math/matrix.h" +#include "vn/xplat/thread.h" +#include "vn/util/compiler.h" +#include "vn/protocol/uart/util.h" + +#if PYTHON + #include "vn/python/util.h" +#endif +#include + +using namespace std; +using namespace vn::math; +using namespace vn::xplat; +using namespace vn::protocol::uart; +using namespace vn::data::integrity; + +#define COMMAND_MAX_LENGTH 0x100 + +namespace vn { +namespace sensors { + +sensor_error::sensor_error() +{ } + +sensor_error::sensor_error(SensorError e) : + exception(), + error(e), + _errorMessage(NULL) +{ } + +sensor_error::sensor_error(sensor_error const& e) : + exception(), + error(e.error), + _errorMessage(NULL) +{ } + +sensor_error::~sensor_error() throw() +{ + if (_errorMessage != NULL) + delete[] _errorMessage; +} + +char const* sensor_error::what() const throw() +{ + if (_errorMessage != NULL) + return _errorMessage; + + string errorMsg = "received sensor error '" + str(error) + "'"; + + const_cast(_errorMessage) = new char[errorMsg.size() + 1]; + + #if VN_HAVE_SECURE_CRT + strcpy_s(_errorMessage, errorMsg.size() + 1, errorMsg.c_str()); + #else + strcpy(_errorMessage, errorMsg.c_str()); + #endif + + return _errorMessage; +} + +struct VnSensor::Impl +{ + static const size_t DefaultReadBufferSize = 256; + static const uint16_t DefaultResponseTimeoutMs = 500; + static const uint16_t DefaultRetransmitDelayMs = 200; + + SerialPort *pSerialPort; + IPort* port; + bool SimplePortIsOurs; + bool DidWeOpenSimplePort; + RawDataReceivedHandler _rawDataReceivedHandler; + void* _rawDataReceivedUserData; + PossiblePacketFoundHandler _possiblePacketFoundHandler; + void* _possiblePacketFoundUserData; + PacketFinder _packetFinder; + size_t _dataRunningIndex; + AsyncPacketReceivedHandler _asyncPacketReceivedHandler; + void* _asyncPacketReceivedUserData; + ErrorDetectionMode _sendErrorDetectionMode; + VnSensor* BackReference; + queue _receivedResponses; + CriticalSection _transactionCS; + bool _waitingForResponse; + ErrorPacketReceivedHandler _errorPacketReceivedHandler; + void* _errorPacketReceivedUserData; + uint16_t _responseTimeoutMs; + uint16_t _retransmitDelayMs; + xplat::Event _newResponsesEvent; + #if PYTHON + PyObject* _rawDataReceivedHandlerPython; + PyObject* _asyncPacketReceivedHandlerPython; + #endif + + explicit Impl(VnSensor* backReference) : + pSerialPort(NULL), + port(NULL), + SimplePortIsOurs(false), + DidWeOpenSimplePort(false), + _rawDataReceivedHandler(NULL), + _rawDataReceivedUserData(NULL), + _possiblePacketFoundHandler(NULL), + _possiblePacketFoundUserData(NULL), + _dataRunningIndex(0), + _asyncPacketReceivedHandler(NULL), + _asyncPacketReceivedUserData(NULL), + _sendErrorDetectionMode(ERRORDETECTIONMODE_CHECKSUM), + BackReference(backReference), + _waitingForResponse(false), + _errorPacketReceivedHandler(NULL), + _errorPacketReceivedUserData(NULL), + _responseTimeoutMs(DefaultResponseTimeoutMs), + _retransmitDelayMs(DefaultRetransmitDelayMs) + #if PYTHON + , + _asyncPacketReceivedHandlerPython(NULL), + _rawDataReceivedHandlerPython(NULL) + #endif + { + _packetFinder.registerPossiblePacketFoundHandler(this, possiblePacketFoundHandler); + } + + ~Impl() + { + _packetFinder.unregisterPossiblePacketFoundHandler(); + } + + void onPossiblePacketFound(Packet& possiblePacket, size_t packetStartRunningIndex) + { + if (_possiblePacketFoundHandler != NULL) + _possiblePacketFoundHandler(_possiblePacketFoundUserData, possiblePacket, packetStartRunningIndex); + } + + void onAsyncPacketReceived(Packet& asciiPacket, size_t runningIndex, TimeStamp timestamp) + { + //cout << "Made A" << flush << endl; + + if (_asyncPacketReceivedHandler != NULL) + _asyncPacketReceivedHandler(_asyncPacketReceivedUserData, asciiPacket, runningIndex); + + #if PYTHON + BackReference->eventAsyncPacketReceived.fire(asciiPacket, runningIndex, timestamp); + #endif + + //#if PYTHON + //if (_asyncPacketReceivedHandlerPython != NULL) + //{ + // python::AcquireGIL scopedLock; + // + // boost::python::call(_asyncPacketReceivedHandlerPython, boost::ref(asciiPacket), runningIndex); + //} + //#endif + } + + void onErrorPacketReceived(Packet& errorPacket, size_t runningIndex) + { + if (_errorPacketReceivedHandler != NULL) + _errorPacketReceivedHandler(_errorPacketReceivedUserData, errorPacket, runningIndex); + } + + static void possiblePacketFoundHandler(void* userData, Packet& possiblePacket, size_t packetStartRunningIndex, TimeStamp timestamp) + { + //cout << "Made A" << flush << endl; + + Impl* pThis = static_cast(userData); + + pThis->onPossiblePacketFound(possiblePacket, packetStartRunningIndex); + + if (!possiblePacket.isValid()) + return; + + if (possiblePacket.isError()) + { + if (pThis->_waitingForResponse) + { + pThis->_transactionCS.enter(); + pThis->_receivedResponses.push(possiblePacket); + pThis->_newResponsesEvent.signal(); + pThis->_transactionCS.leave(); + } + + pThis->onErrorPacketReceived(possiblePacket, packetStartRunningIndex); + + return; + } + + if (possiblePacket.isResponse() && pThis->_waitingForResponse) + { + pThis->_transactionCS.enter(); + pThis->_receivedResponses.push(possiblePacket); + pThis->_newResponsesEvent.signal(); + pThis->_transactionCS.leave(); + + return; + } + + // This wasn't anything else. We assume it is an async packet. + pThis->onAsyncPacketReceived(possiblePacket, packetStartRunningIndex, timestamp); + } + + static void dataReceivedHandler(void* userData) + { + static char readBuffer[DefaultReadBufferSize]; + Impl *pi = static_cast(userData); + + size_t numOfBytesRead = 0; + + pi->port->read( + readBuffer, + DefaultReadBufferSize, + numOfBytesRead); + + if (numOfBytesRead == 0) + return; + + TimeStamp t = TimeStamp::get(); + + if (pi->_rawDataReceivedHandler != NULL) + pi->_rawDataReceivedHandler(pi->_rawDataReceivedUserData, reinterpret_cast(readBuffer), numOfBytesRead, pi->_dataRunningIndex); + + #if PYTHON + if (pi->_rawDataReceivedHandlerPython != NULL) + { + vector pRawData(readBuffer, readBuffer + numOfBytesRead); + + python::AcquireGIL scopedLock; + + boost::python::call(pi->_rawDataReceivedHandlerPython, pRawData, pi->_dataRunningIndex); + } + #endif + + pi->_packetFinder.processReceivedData(reinterpret_cast(readBuffer), numOfBytesRead, t); + + pi->_dataRunningIndex += numOfBytesRead; + } + + bool isConnected() + { + return port != NULL && port->isOpen(); + } + + size_t finalizeCommandToSend(char *toSend, size_t length) + { + #if defined(_MSC_VER) + // Unable to use save version 'sprintf_s' since the length of 'toSend' is unknown here. + #pragma warning(push) + #pragma warning(disable:4996) + #endif + + if (_sendErrorDetectionMode == ERRORDETECTIONMODE_CHECKSUM) + { + length += sprintf(toSend + length, "%02X\r\n", Checksum8::compute(toSend + 1, length - 2)); + } + else if (_sendErrorDetectionMode == ERRORDETECTIONMODE_CRC) + { + length += sprintf(toSend + length, "%04X\r\n", Crc16::compute(toSend + 1, length - 2)); + } + else + { + length += sprintf(toSend + length, "XX\r\n"); + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif + + return length; + } + + Packet transactionWithWait(char* toSend, size_t length, uint16_t responseTimeoutMs, uint16_t retransmitDelayMs) + { + // Make sure we don't have any existing responses. + _transactionCS.enter(); + + #if VN_SUPPORTS_SWAP + queue empty; + _receivedResponses.swap(empty); + #else + while (!_receivedResponses.empty()) _receivedResponses.pop(); + #endif + _waitingForResponse = true; + _transactionCS.leave(); + + // Send the command and continue sending if retransmits are enabled + // until we receive the response or timeout. + Stopwatch timeoutSw; + + port->write(toSend, length); + float curElapsedTime = timeoutSw.elapsedMs(); + + while (true) + { + bool shouldRetransmit = false; + + // Compute how long we should wait for a response before taking + // more action. + float responseWaitTime = responseTimeoutMs - curElapsedTime; + if (responseWaitTime > retransmitDelayMs) + { + responseWaitTime = retransmitDelayMs; + shouldRetransmit = true; + } + + // See if we have time left. + if (responseWaitTime < 0) + { + _waitingForResponse = false; + throw timeout(); + } + + // Wait for any new responses that come in or until it is time to + // send a new retransmit. + xplat::Event::WaitResult waitResult = _newResponsesEvent.waitUs(static_cast(responseWaitTime * 1000)); + + queue responsesToProcess; + + if (waitResult == xplat::Event::WAIT_TIMEDOUT) + { + if (!shouldRetransmit) + { + _waitingForResponse = false; + throw timeout(); + } + } + + if (waitResult == xplat::Event::WAIT_SIGNALED) + { + // Get the current collection of responses. + _transactionCS.enter(); + #if VN_SUPPORTS_SWAP + _receivedResponses.swap(responsesToProcess); + #else + while (!_receivedResponses.empty()) + { + responsesToProcess.push(_receivedResponses.front()); + _receivedResponses.pop(); + } + #endif + _transactionCS.leave(); + } + + // Process the collection of responses we have. + while (!responsesToProcess.empty()) + { + Packet p = responsesToProcess.front(); + responsesToProcess.pop(); + + if (p.isError()) + { + _waitingForResponse = false; + throw sensor_error(p.parseError()); + } + + // We must have a response packet. + _waitingForResponse = false; + return p; + } + + // Retransmit. + port->write(toSend, length); + curElapsedTime = timeoutSw.elapsedMs(); + } + } + + void transactionNoFinalize(char* toSend, size_t length, bool waitForReply, Packet *response, uint16_t responseTimeoutMs, uint16_t retransmitDelayMs) + { + if (!isConnected()) + throw invalid_operation(); + + if (waitForReply) + { + *response = transactionWithWait(toSend, length, responseTimeoutMs, retransmitDelayMs); + + if (response->isError()) + throw sensor_error(response->parseError()); + } + else + { + port->write(toSend, length); + } + } + + void transactionNoFinalize(char* toSend, size_t length, bool waitForReply, Packet *response) + { + transactionNoFinalize(toSend, length, waitForReply, response, _responseTimeoutMs, _retransmitDelayMs); + } + + void transaction(char* toSend, size_t length, bool waitForReply, Packet *response, uint16_t responseTimeoutMs, uint16_t retransmitDelayMs) + { + length = finalizeCommandToSend(toSend, length); + + transactionNoFinalize(toSend, length, waitForReply, response, responseTimeoutMs, retransmitDelayMs); + } + + // Performs a communication transaction with the sensor. If waitForReply is + // set to true, we will retransmit the message until we receive a response + // or until we time out, depending on current settings. + void transaction(char* toSend, size_t length, bool waitForReply, Packet *response) + { + transaction(toSend, length, waitForReply, response, _responseTimeoutMs, _retransmitDelayMs); + } + + BinaryOutputRegister readBinaryOutput(uint8_t binaryOutputNumber) + { + char toSend[17]; + Packet response; + uint16_t asyncMode, rateDivisor, outputGroup, commonField, timeField, imuField, gpsField, attitudeField, insField; + + #if VN_HAVE_SECURE_CRT + int length = sprintf_s(toSend, sizeof(toSend), "$VNRRG,%u*", 74 + binaryOutputNumber); + #else + int length = sprintf(toSend, "$VNRRG,%u*", 74 + binaryOutputNumber); + #endif + + transaction(toSend, length, true, &response); + + response.parseBinaryOutput( + &asyncMode, + &rateDivisor, + &outputGroup, + &commonField, + &timeField, + &imuField, + &gpsField, + &attitudeField, + &insField); + + return BinaryOutputRegister( + static_cast(asyncMode), + rateDivisor, + static_cast(commonField), + static_cast(timeField), + static_cast(imuField), + static_cast(gpsField), + static_cast(attitudeField), + static_cast(insField)); + } + + void writeBinaryOutput(uint8_t binaryOutputNumber, BinaryOutputRegister &fields, bool waitForReply) + { + char toSend[256]; + Packet response; + + // First determine which groups are present. + uint16_t groups = 0; + if (fields.commonField) + groups |= 0x0001; + if (fields.timeField) + groups |= 0x0002; + if (fields.imuField) + groups |= 0x0004; + if (fields.gpsField) + groups |= 0x0008; + if (fields.attitudeField) + groups |= 0x0010; + if (fields.insField) + groups |= 0x0020; + + #if VN_HAVE_SECURE_CRT + int length = sprintf_s(toSend, sizeof(toSend), "$VNWRG,%u,%u,%u,%X", 74 + binaryOutputNumber, fields.asyncMode, fields.rateDivisor, groups); + #else + int length = sprintf(toSend, "$VNWRG,%u,%u,%u,%X", 74 + binaryOutputNumber, fields.asyncMode, fields.rateDivisor, groups); + #endif + + if (fields.commonField) + #if VN_HAVE_SECURE_CRT + length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.commonField); + #else + length += sprintf(toSend + length, ",%X", fields.commonField); + #endif + if (fields.timeField) + #if VN_HAVE_SECURE_CRT + length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.timeField); + #else + length += sprintf(toSend + length, ",%X", fields.timeField); + #endif + if (fields.imuField) + #if VN_HAVE_SECURE_CRT + length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.imuField); + #else + length += sprintf(toSend + length, ",%X", fields.imuField); + #endif + if (fields.gpsField) + #if VN_HAVE_SECURE_CRT + length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.gpsField); + #else + length += sprintf(toSend + length, ",%X", fields.gpsField); + #endif + if (fields.attitudeField) + #if VN_HAVE_SECURE_CRT + length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.attitudeField); + #else + length += sprintf(toSend + length, ",%X", fields.attitudeField); + #endif + if (fields.insField) + #if VN_HAVE_SECURE_CRT + length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.insField); + #else + length += sprintf(toSend + length, ",%X", fields.insField); + #endif + + #if VN_HAVE_SECURE_CRT + length += sprintf_s(toSend + length, sizeof(toSend) - length, "*"); + #else + length += sprintf(toSend + length, "*"); + #endif + + transaction(toSend, length, waitForReply, &response); + } +}; + +vector VnSensor::supportedBaudrates() +{ + uint32_t br[] = { + 9600, + 19200, + 38400, + 57600, + 115200, + 128000, + 230400, + 460800, + 921600 }; + + return vector(br, br + sizeof(br) / sizeof(uint32_t)); +} + +#if defined(_MSC_VER) && _MSC_VER <= 1600 + #pragma warning(push) + // Disable VS2010 warning for 'this' used in base member initializer list. + #pragma warning(disable:4355) +#endif + +VnSensor::VnSensor() : + _pi(new Impl(this)) +{ +} + +#if defined(_MSC_VER) && _MSC_VER <= 1600 + #pragma warning(pop) +#endif + +VnSensor::~VnSensor() +{ + if (_pi->SimplePortIsOurs && _pi->DidWeOpenSimplePort && isConnected()) + disconnect(); + + delete _pi; +} + +uint32_t VnSensor::baudrate() +{ + if (_pi->pSerialPort == NULL) + // We are not connected to a known serial port. + throw invalid_operation(); + + return _pi->pSerialPort->baudrate(); +} + +/*void VnSensor::changeBaudrate(uint32_t br) +{ + if (_pi->pSerialPort == NULL) + // We are not connected to a known serial port. + throw invalid_operation(); + + _pi->pSerialPort->changeBaudrate(br); +}*/ + +ErrorDetectionMode VnSensor::sendErrorDetectionMode() +{ + return _pi->_sendErrorDetectionMode; +} + +void VnSensor::setSendErrorDetectionMode(ErrorDetectionMode mode) +{ + _pi->_sendErrorDetectionMode = mode; +} + +bool VnSensor::isConnected() +{ + return _pi->isConnected(); +} + +uint16_t VnSensor::responseTimeoutMs() +{ + return _pi->_responseTimeoutMs; +} + +void VnSensor::setResponseTimeoutMs(uint16_t timeout) +{ + _pi->_responseTimeoutMs = timeout; +} + +uint16_t VnSensor::retransmitDelayMs() +{ + return _pi->_retransmitDelayMs; +} + +void VnSensor::setRetransmitDelayMs(uint16_t delay) +{ + _pi->_retransmitDelayMs = delay; +} + +bool VnSensor::verifySensorConnectivity() +{ + try + { + readModelNumber(); + + return true; + } + catch (...) { } + + return false; +} + +void VnSensor::connect(const string &portName, uint32_t baudrate) +{ + _pi->pSerialPort = new SerialPort(portName, baudrate); + + connect(dynamic_cast(_pi->pSerialPort)); + + _pi->SimplePortIsOurs = true; +} + +void VnSensor::connect(IPort* simplePort) +{ + _pi->port = simplePort; + _pi->SimplePortIsOurs = false; + + _pi->port->registerDataReceivedHandler(_pi, Impl::dataReceivedHandler); + + if (!_pi->port->isOpen()) + { + _pi->port->open(); + _pi->DidWeOpenSimplePort = true; + } +} + +void VnSensor::disconnect() +{ + if (_pi->port == NULL || !_pi->port->isOpen()) + throw invalid_operation(); + + _pi->port->unregisterDataReceivedHandler(); + + if (_pi->DidWeOpenSimplePort) + { + _pi->port->close(); + } + + _pi->DidWeOpenSimplePort = false; + + if (_pi->SimplePortIsOurs) + { + delete _pi->port; + + _pi->port = NULL; + } +} + +string VnSensor::transaction(string toSend) +{ + char buffer[COMMAND_MAX_LENGTH]; + size_t finalLength = toSend.length(); + Packet response; + + // Copy over what was provided. + copy(toSend.begin(), toSend.end(), buffer); + + // First see if an '*' is present. + if (toSend.find('*') == string::npos) + { + buffer[toSend.length()] = '*'; + finalLength = _pi->finalizeCommandToSend(buffer, toSend.length() + 1); + } + else if (toSend[toSend.length() - 2] != '\r' && toSend[toSend.length() - 1] != '\n') + { + buffer[toSend.length()] = '\r'; + buffer[toSend.length() + 1] = '\n'; + finalLength += 2; + } + + _pi->transactionNoFinalize(buffer, finalLength, true, &response); + + return response.datastr(); +} + +string VnSensor::send(string toSend, bool waitForReply, ErrorDetectionMode errorDetectionMode) +{ + Packet p; + char *buffer = new char[toSend.size() + 8]; // Extra room for possible additions. + size_t curToSendLength = toSend.size(); + + // See if a '$' needs to be prepended. + if (toSend[0] == '$') + { + #if VN_HAVE_SECURE_SCL + toSend._Copy_s(buffer, toSend.size() + 8, toSend.size()); + #else + toSend.copy(buffer, toSend.size()); + #endif + + } + else + { + buffer[0] = '$'; + #if VN_HAVE_SECURE_SCL + toSend._Copy_s(buffer + 1, toSend.size() + 8 - 1, toSend.size()); + #else + toSend.copy(buffer + 1, toSend.size()); + #endif + curToSendLength++; + } + + // Locate any '*' in the command. + size_t astrickLocation = string::npos; + for (size_t i = 0; i < curToSendLength; i++) + { + if (buffer[i] == '*') + { + astrickLocation = i; + break; + } + } + + // Do we need to add a '*'? + if (astrickLocation == string::npos) + { + buffer[curToSendLength] = '*'; + astrickLocation = curToSendLength++; + } + + // Do we need to add a checksum/CRC? + if (astrickLocation == curToSendLength - 1) + { + if (errorDetectionMode == ERRORDETECTIONMODE_CHECKSUM) + { + #if VN_HAVE_SECURE_CRT + curToSendLength += sprintf_s(buffer + curToSendLength, toSend.size() + 8 - curToSendLength, "%02X\r\n", Checksum8::compute(buffer + 1, curToSendLength - 2)); + #else + curToSendLength += sprintf(buffer + curToSendLength, "%02X\r\n", Checksum8::compute(buffer + 1, curToSendLength - 2)); + #endif + } + else if (errorDetectionMode == ERRORDETECTIONMODE_CRC) + { + #if VN_HAVE_SECURE_CRT + curToSendLength += sprintf_s(buffer + curToSendLength, toSend.size() + 8 - curToSendLength, "%04X\r\n", Crc16::compute(buffer + 1, curToSendLength - 2)); + #else + curToSendLength += sprintf(buffer + curToSendLength, "%04X\r\n", Crc16::compute(buffer + 1, curToSendLength - 2)); + #endif + } + else + { + #if VN_HAVE_SECURE_CRT + curToSendLength += sprintf_s(buffer + curToSendLength, toSend.size() + 8 - curToSendLength, "*XX\r\n"); + #else + curToSendLength += sprintf(buffer + curToSendLength, "*XX\r\n"); + #endif + } + } + // Do we need to add "\r\n"? + else if (buffer[curToSendLength - 1] != '\n') + { + buffer[curToSendLength++] = '\r'; + buffer[curToSendLength++] = '\n'; + } + + _pi->transactionNoFinalize(buffer, curToSendLength, waitForReply, &p, _pi->_responseTimeoutMs, _pi->_retransmitDelayMs); + + delete [] buffer; + + return p.datastr(); +} + +void VnSensor::registerRawDataReceivedHandler(void* userData, RawDataReceivedHandler handler) +{ + if (_pi->_rawDataReceivedHandler != NULL) + throw invalid_operation(); + + _pi->_rawDataReceivedHandler = handler; + _pi->_rawDataReceivedUserData = userData; +} + +#if PL150 +#else + +#if PYTHON + +PyObject* VnSensor::registerRawDataReceivedHandler(PyObject* callable) +{ + _pi->_rawDataReceivedHandlerPython = callable; + callable->ob_refcnt++; + + return Py_None; +} + +#endif +#endif + +void VnSensor::unregisterRawDataReceivedHandler() +{ + if (_pi->_rawDataReceivedHandler == NULL) + throw invalid_operation(); + + _pi->_rawDataReceivedHandler = NULL; + _pi->_rawDataReceivedUserData = NULL; +} + +void VnSensor::registerPossiblePacketFoundHandler(void* userData, PossiblePacketFoundHandler handler) +{ + if (_pi->_possiblePacketFoundHandler != NULL) + throw invalid_operation(); + + _pi->_possiblePacketFoundHandler = handler; + _pi->_possiblePacketFoundUserData = userData; +} + +void VnSensor::unregisterPossiblePacketFoundHandler() +{ + if (_pi->_possiblePacketFoundHandler == NULL) + throw invalid_operation(); + + _pi->_possiblePacketFoundHandler = NULL; + _pi->_possiblePacketFoundUserData = NULL; +} + +void VnSensor::registerAsyncPacketReceivedHandler(void* userData, AsyncPacketReceivedHandler handler) +{ + if (_pi->_asyncPacketReceivedHandler != NULL) + throw invalid_operation(); + + _pi->_asyncPacketReceivedHandler = handler; + _pi->_asyncPacketReceivedUserData = userData; +} + +#if PL150 + +#else +#if PYTHON + +PyObject* VnSensor::registerAsyncPacketReceivedHandler(PyObject* callable) +{ + _pi->_asyncPacketReceivedHandlerPython = callable; + callable->ob_refcnt++; + + return Py_None; +} + +#endif +#endif + +void VnSensor::unregisterAsyncPacketReceivedHandler() +{ + if (_pi->_asyncPacketReceivedHandler == NULL) + throw invalid_operation(); + + _pi->_asyncPacketReceivedHandler = NULL; + _pi->_asyncPacketReceivedUserData = NULL; +} + +void VnSensor::registerErrorPacketReceivedHandler(void* userData, ErrorPacketReceivedHandler handler) +{ + if (_pi->_errorPacketReceivedHandler != NULL) + throw invalid_operation(); + + _pi->_errorPacketReceivedHandler = handler; + _pi->_errorPacketReceivedUserData = userData; +} + +void VnSensor::unregisterErrorPacketReceivedHandler() +{ + if (_pi->_errorPacketReceivedHandler == NULL) + throw invalid_operation(); + + _pi->_errorPacketReceivedHandler = NULL; + _pi->_errorPacketReceivedUserData = NULL; +} + +void VnSensor::writeSettings(bool waitForReply) +{ + char toSend[37]; + + size_t length = Packet::genWriteSettings(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + + // Write settings sometimes takes a while to do and receive a response + // from the sensor. + _pi->transactionNoFinalize(toSend, length, waitForReply, &response, 2500, 1000); +} + +void VnSensor::tare(bool waitForReply) +{ + char toSend[14]; + + size_t length = Packet::genTare(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::setGyroBias(bool waitForReply) +{ + char toSend[14]; + + size_t length = Packet::genSetGyroBias(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::magneticDisturbancePresent(bool disturbancePresent, bool waitForReply) +{ + char toSend[16]; + + size_t length = Packet::genKnownMagneticDisturbance(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), disturbancePresent); + + Packet response; + + // Write settings sometimes takes a while to do and receive a response + // from the sensor. + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::accelerationDisturbancePresent(bool disturbancePresent, bool waitForReply) +{ + char toSend[16]; + + size_t length = Packet::genKnownAccelerationDisturbance(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), disturbancePresent); + + Packet response; + + // Write settings sometimes takes a while to do and receive a response + // from the sensor. + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::restoreFactorySettings(bool waitForReply) +{ + char toSend[37]; + + size_t length = Packet::genRestoreFactorySettings(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + + // Restore factory settings sometimes takes a while to do and receive a + // response from the sensor. + _pi->transactionNoFinalize(toSend, length, waitForReply, &response, 2500, 1000); +} + +void VnSensor::reset(bool waitForReply) +{ + char toSend[37]; + + size_t length = Packet::genReset(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + + // Reset sometimes takes a while to do and receive a response from the + // sensor. + _pi->transactionNoFinalize(toSend, length, waitForReply, &response, 2500, 1000); +} + +void VnSensor::changeBaudRate(uint32_t baudrate) +{ + writeSerialBaudRate(baudrate, true); + + _pi->pSerialPort->changeBaudrate(baudrate); +} + +VnSensor::Family VnSensor::determineDeviceFamily() +{ + string mn = readModelNumber(); + + return determineDeviceFamily(mn); +} + +VnSensor::Family VnSensor::determineDeviceFamily(std::string modelNumber) +{ + if (modelNumber.find("VN-100") == 0) + return VnSensor_Family_Vn100; + + if (modelNumber.find("VN-200") == 0) + return VnSensor_Family_Vn200; + + if (modelNumber.find("VN-300") == 0) + return VnSensor_Family_Vn300; + + return VnSensor_Family_Unknown; +} + +BinaryOutputRegister VnSensor::readBinaryOutput1() +{ + return _pi->readBinaryOutput(1); +} + +BinaryOutputRegister VnSensor::readBinaryOutput2() +{ + return _pi->readBinaryOutput(2); +} + +BinaryOutputRegister VnSensor::readBinaryOutput3() +{ + return _pi->readBinaryOutput(3); +} + + +void VnSensor::writeBinaryOutput1(BinaryOutputRegister &fields, bool waitForReply) +{ + _pi->writeBinaryOutput(1, fields, waitForReply); +} + +void VnSensor::writeBinaryOutput2(BinaryOutputRegister &fields, bool waitForReply) +{ + _pi->writeBinaryOutput(2, fields, waitForReply); +} + +void VnSensor::writeBinaryOutput3(BinaryOutputRegister &fields, bool waitForReply) +{ + _pi->writeBinaryOutput(3, fields, waitForReply); +} + + +uint32_t VnSensor::readSerialBaudRate(uint8_t port) +{ + char toSend[17]; + + size_t length = Packet::genReadSerialBaudRate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), port); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + uint32_t reg; + response.parseSerialBaudRate(®); + + return reg; +} + +void VnSensor::writeSerialBaudRate(const uint32_t &baudrate, uint8_t port, bool waitForReply) +{ + char toSend[25]; + + size_t length = Packet::genWriteSerialBaudRate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), baudrate, port); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +AsciiAsync VnSensor::readAsyncDataOutputType(uint8_t port) +{ + char toSend[17]; + + size_t length = Packet::genReadAsyncDataOutputType(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), port); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + uint32_t reg; + response.parseAsyncDataOutputType(®); + + return static_cast(reg); +} + +void VnSensor::writeAsyncDataOutputType(AsciiAsync ador, uint8_t port, bool waitForReply) +{ + char toSend[21]; + + size_t length = Packet::genWriteAsyncDataOutputType(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), ador, port); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +uint32_t VnSensor::readAsyncDataOutputFrequency(uint8_t port) +{ + char toSend[17]; + + size_t length = Packet::genReadAsyncDataOutputFrequency(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), port); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + uint32_t reg; + response.parseAsyncDataOutputFrequency(®); + + return reg; +} + +void VnSensor::writeAsyncDataOutputFrequency(const uint32_t &adof, uint8_t port, bool waitForReply) +{ + char toSend[26]; + + size_t length = Packet::genWriteAsyncDataOutputFrequency(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), adof, port); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +InsBasicConfigurationRegisterVn200 VnSensor::readInsBasicConfigurationVn200() +{ + char toSend[17]; + + size_t length = Packet::genReadInsBasicConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + InsBasicConfigurationRegisterVn200 reg; + uint8_t scenario; + uint8_t ahrsAiding; + response.parseInsBasicConfiguration( + &scenario, + &ahrsAiding); + + reg.scenario = static_cast(scenario); + reg.ahrsAiding = ahrsAiding != 0; + + return reg; +} + +void VnSensor::writeInsBasicConfigurationVn200(InsBasicConfigurationRegisterVn200 &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteInsBasicConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.scenario, fields.ahrsAiding, 0); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeInsBasicConfigurationVn200( + Scenario scenario, + const uint8_t &ahrsAiding, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteInsBasicConfiguration( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + scenario, + ahrsAiding, + 0); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +InsBasicConfigurationRegisterVn300 VnSensor::readInsBasicConfigurationVn300() +{ + char toSend[17]; + + size_t length = Packet::genReadInsBasicConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + InsBasicConfigurationRegisterVn300 reg; + uint8_t scenario; + uint8_t ahrsAiding; + uint8_t estBaseline; + response.parseInsBasicConfiguration( + &scenario, + &ahrsAiding, + &estBaseline); + + reg.scenario = static_cast(scenario); + reg.ahrsAiding = ahrsAiding != 0; + reg.estBaseline = estBaseline != 0; + + return reg; +} + +void VnSensor::writeInsBasicConfigurationVn300(InsBasicConfigurationRegisterVn300 &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteInsBasicConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.scenario, fields.ahrsAiding, fields.estBaseline); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeInsBasicConfigurationVn300( + Scenario scenario, + const uint8_t &ahrsAiding, + const uint8_t &estBaseline, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteInsBasicConfiguration( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + scenario, + ahrsAiding, + estBaseline); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +string VnSensor::readUserTag() +{ + char toSend[17]; + + size_t length = Packet::genReadUserTag(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + char tagBuffer[50]; + response.parseUserTag(tagBuffer); + + return tagBuffer; +} + +void VnSensor::writeUserTag(const string &tag, bool waitForReply) +{ + char toSend[37]; + + size_t length = Packet::genWriteUserTag(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), tag.c_str()); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +string VnSensor::readModelNumber() +{ + char toSend[17]; + + size_t length = Packet::genReadModelNumber(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + char productNameBuffer[50]; + response.parseModelNumber(productNameBuffer); + + return productNameBuffer; +} + +uint32_t VnSensor::readHardwareRevision() +{ + char toSend[17]; + + size_t length = Packet::genReadHardwareRevision(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + uint32_t reg; + response.parseHardwareRevision(®); + + return reg; +} + +uint32_t VnSensor::readSerialNumber() +{ + char toSend[17]; + + size_t length = Packet::genReadSerialNumber(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + uint32_t reg; + response.parseSerialNumber(®); + + return reg; +} + +string VnSensor::readFirmwareVersion() +{ + char toSend[17]; + + size_t length = Packet::genReadFirmwareVersion(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + char firmwareVersionBuffer[50]; + response.parseFirmwareVersion(firmwareVersionBuffer); + + return firmwareVersionBuffer; +} + +uint32_t VnSensor::readSerialBaudRate() +{ + char toSend[17]; + + size_t length = Packet::genReadSerialBaudRate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + uint32_t reg; + response.parseSerialBaudRate(®); + + return reg; +} + +void VnSensor::writeSerialBaudRate(const uint32_t &baudrate, bool waitForReply) +{ + char toSend[25]; + + size_t length = Packet::genWriteSerialBaudRate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), baudrate); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +AsciiAsync VnSensor::readAsyncDataOutputType() +{ + char toSend[17]; + + size_t length = Packet::genReadAsyncDataOutputType(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + uint32_t reg; + response.parseAsyncDataOutputType(®); + + return static_cast(reg); +} + +void VnSensor::writeAsyncDataOutputType(AsciiAsync ador, bool waitForReply) +{ + char toSend[19]; + + size_t length = Packet::genWriteAsyncDataOutputType(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), ador); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +uint32_t VnSensor::readAsyncDataOutputFrequency() +{ + char toSend[17]; + + size_t length = Packet::genReadAsyncDataOutputFrequency(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + uint32_t reg; + response.parseAsyncDataOutputFrequency(®); + + return reg; +} + +void VnSensor::writeAsyncDataOutputFrequency(const uint32_t &adof, bool waitForReply) +{ + char toSend[26]; + + size_t length = Packet::genWriteAsyncDataOutputFrequency(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), adof); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +vec3f VnSensor::readYawPitchRoll() +{ + char toSend[17]; + + size_t length = Packet::genReadYawPitchRoll(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + vec3f reg; + response.parseYawPitchRoll(®); + + return reg; +} + +vec4f VnSensor::readAttitudeQuaternion() +{ + char toSend[17]; + + size_t length = Packet::genReadAttitudeQuaternion(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + vec4f reg; + response.parseAttitudeQuaternion(®); + + return reg; +} + +QuaternionMagneticAccelerationAndAngularRatesRegister VnSensor::readQuaternionMagneticAccelerationAndAngularRates() +{ + char toSend[17]; + + size_t length = Packet::genReadQuaternionMagneticAccelerationAndAngularRates(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + QuaternionMagneticAccelerationAndAngularRatesRegister reg; + response.parseQuaternionMagneticAccelerationAndAngularRates( + ®.quat, + ®.mag, + ®.accel, + ®.gyro); + + return reg; +} + +vec3f VnSensor::readMagneticMeasurements() +{ + char toSend[17]; + + size_t length = Packet::genReadMagneticMeasurements(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + vec3f reg; + response.parseMagneticMeasurements(®); + + return reg; +} + +vec3f VnSensor::readAccelerationMeasurements() +{ + char toSend[17]; + + size_t length = Packet::genReadAccelerationMeasurements(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + vec3f reg; + response.parseAccelerationMeasurements(®); + + return reg; +} + +vec3f VnSensor::readAngularRateMeasurements() +{ + char toSend[17]; + + size_t length = Packet::genReadAngularRateMeasurements(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + vec3f reg; + response.parseAngularRateMeasurements(®); + + return reg; +} + +MagneticAccelerationAndAngularRatesRegister VnSensor::readMagneticAccelerationAndAngularRates() +{ + char toSend[17]; + + size_t length = Packet::genReadMagneticAccelerationAndAngularRates(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + MagneticAccelerationAndAngularRatesRegister reg; + response.parseMagneticAccelerationAndAngularRates( + ®.mag, + ®.accel, + ®.gyro); + + return reg; +} + +MagneticAndGravityReferenceVectorsRegister VnSensor::readMagneticAndGravityReferenceVectors() +{ + char toSend[17]; + + size_t length = Packet::genReadMagneticAndGravityReferenceVectors(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + MagneticAndGravityReferenceVectorsRegister reg; + response.parseMagneticAndGravityReferenceVectors( + ®.magRef, + ®.accRef); + + return reg; +} + +void VnSensor::writeMagneticAndGravityReferenceVectors(MagneticAndGravityReferenceVectorsRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteMagneticAndGravityReferenceVectors(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.magRef, fields.accRef); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeMagneticAndGravityReferenceVectors( + const vec3f &magRef, + const vec3f &accRef, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteMagneticAndGravityReferenceVectors( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + magRef, + accRef); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +FilterMeasurementsVarianceParametersRegister VnSensor::readFilterMeasurementsVarianceParameters() +{ + char toSend[17]; + + size_t length = Packet::genReadFilterMeasurementsVarianceParameters(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + FilterMeasurementsVarianceParametersRegister reg; + response.parseFilterMeasurementsVarianceParameters( + ®.angularWalkVariance, + ®.angularRateVariance, + ®.magneticVariance, + ®.accelerationVariance); + + return reg; +} + +void VnSensor::writeFilterMeasurementsVarianceParameters(FilterMeasurementsVarianceParametersRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteFilterMeasurementsVarianceParameters(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.angularWalkVariance, fields.angularRateVariance, fields.magneticVariance, fields.accelerationVariance); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeFilterMeasurementsVarianceParameters( + const float &angularWalkVariance, + const vec3f &angularRateVariance, + const vec3f &magneticVariance, + const vec3f &accelerationVariance, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteFilterMeasurementsVarianceParameters( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + angularWalkVariance, + angularRateVariance, + magneticVariance, + accelerationVariance); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +MagnetometerCompensationRegister VnSensor::readMagnetometerCompensation() +{ + char toSend[17]; + + size_t length = Packet::genReadMagnetometerCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + MagnetometerCompensationRegister reg; + response.parseMagnetometerCompensation( + ®.c, + ®.b); + + return reg; +} + +void VnSensor::writeMagnetometerCompensation(MagnetometerCompensationRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteMagnetometerCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.c, fields.b); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeMagnetometerCompensation( + const mat3f &c, + const vec3f &b, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteMagnetometerCompensation( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + c, + b); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +FilterActiveTuningParametersRegister VnSensor::readFilterActiveTuningParameters() +{ + char toSend[17]; + + size_t length = Packet::genReadFilterActiveTuningParameters(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + FilterActiveTuningParametersRegister reg; + response.parseFilterActiveTuningParameters( + ®.magneticDisturbanceGain, + ®.accelerationDisturbanceGain, + ®.magneticDisturbanceMemory, + ®.accelerationDisturbanceMemory); + + return reg; +} + +void VnSensor::writeFilterActiveTuningParameters(FilterActiveTuningParametersRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteFilterActiveTuningParameters(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.magneticDisturbanceGain, fields.accelerationDisturbanceGain, fields.magneticDisturbanceMemory, fields.accelerationDisturbanceMemory); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeFilterActiveTuningParameters( + const float &magneticDisturbanceGain, + const float &accelerationDisturbanceGain, + const float &magneticDisturbanceMemory, + const float &accelerationDisturbanceMemory, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteFilterActiveTuningParameters( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + magneticDisturbanceGain, + accelerationDisturbanceGain, + magneticDisturbanceMemory, + accelerationDisturbanceMemory); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +AccelerationCompensationRegister VnSensor::readAccelerationCompensation() +{ + char toSend[17]; + + size_t length = Packet::genReadAccelerationCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + AccelerationCompensationRegister reg; + response.parseAccelerationCompensation( + ®.c, + ®.b); + + return reg; +} + +void VnSensor::writeAccelerationCompensation(AccelerationCompensationRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteAccelerationCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.c, fields.b); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeAccelerationCompensation( + const mat3f &c, + const vec3f &b, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteAccelerationCompensation( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + c, + b); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +mat3f VnSensor::readReferenceFrameRotation() +{ + char toSend[17]; + + size_t length = Packet::genReadReferenceFrameRotation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + mat3f reg; + response.parseReferenceFrameRotation(®); + + return reg; +} + +void VnSensor::writeReferenceFrameRotation(const mat3f &c, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteReferenceFrameRotation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), c); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +YawPitchRollMagneticAccelerationAndAngularRatesRegister VnSensor::readYawPitchRollMagneticAccelerationAndAngularRates() +{ + char toSend[17]; + + size_t length = Packet::genReadYawPitchRollMagneticAccelerationAndAngularRates(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + YawPitchRollMagneticAccelerationAndAngularRatesRegister reg; + response.parseYawPitchRollMagneticAccelerationAndAngularRates( + ®.yawPitchRoll, + ®.mag, + ®.accel, + ®.gyro); + + return reg; +} + +CommunicationProtocolControlRegister VnSensor::readCommunicationProtocolControl() +{ + char toSend[17]; + + size_t length = Packet::genReadCommunicationProtocolControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + CommunicationProtocolControlRegister reg; + uint8_t serialCount; + uint8_t serialStatus; + uint8_t spiCount; + uint8_t spiStatus; + uint8_t serialChecksum; + uint8_t spiChecksum; + uint8_t errorMode; + response.parseCommunicationProtocolControl( + &serialCount, + &serialStatus, + &spiCount, + &spiStatus, + &serialChecksum, + &spiChecksum, + &errorMode); + + reg.serialCount = static_cast(serialCount); + reg.serialStatus = static_cast(serialStatus); + reg.spiCount = static_cast(spiCount); + reg.spiStatus = static_cast(spiStatus); + reg.serialChecksum = static_cast(serialChecksum); + reg.spiChecksum = static_cast(spiChecksum); + reg.errorMode = static_cast(errorMode); + + return reg; +} + +void VnSensor::writeCommunicationProtocolControl(CommunicationProtocolControlRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteCommunicationProtocolControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.serialCount, fields.serialStatus, fields.spiCount, fields.spiStatus, fields.serialChecksum, fields.spiChecksum, fields.errorMode); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeCommunicationProtocolControl( + CountMode serialCount, + StatusMode serialStatus, + CountMode spiCount, + StatusMode spiStatus, + ChecksumMode serialChecksum, + ChecksumMode spiChecksum, + ErrorMode errorMode, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteCommunicationProtocolControl( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + serialCount, + serialStatus, + spiCount, + spiStatus, + serialChecksum, + spiChecksum, + errorMode); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +SynchronizationControlRegister VnSensor::readSynchronizationControl() +{ + char toSend[17]; + + size_t length = Packet::genReadSynchronizationControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + SynchronizationControlRegister reg; + uint8_t syncInMode; + uint8_t syncInEdge; + uint8_t syncOutMode; + uint8_t syncOutPolarity; + response.parseSynchronizationControl( + &syncInMode, + &syncInEdge, + ®.syncInSkipFactor, + &syncOutMode, + &syncOutPolarity, + ®.syncOutSkipFactor, + ®.syncOutPulseWidth); + + reg.syncInMode = static_cast(syncInMode); + reg.syncInEdge = static_cast(syncInEdge); + reg.syncOutMode = static_cast(syncOutMode); + reg.syncOutPolarity = static_cast(syncOutPolarity); + + return reg; +} + +void VnSensor::writeSynchronizationControl(SynchronizationControlRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteSynchronizationControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.syncInMode, fields.syncInEdge, fields.syncInSkipFactor, fields.syncOutMode, fields.syncOutPolarity, fields.syncOutSkipFactor, fields.syncOutPulseWidth); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeSynchronizationControl( + SyncInMode syncInMode, + SyncInEdge syncInEdge, + const uint16_t &syncInSkipFactor, + SyncOutMode syncOutMode, + SyncOutPolarity syncOutPolarity, + const uint16_t &syncOutSkipFactor, + const uint32_t &syncOutPulseWidth, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteSynchronizationControl( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + syncInMode, + syncInEdge, + syncInSkipFactor, + syncOutMode, + syncOutPolarity, + syncOutSkipFactor, + syncOutPulseWidth); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +SynchronizationStatusRegister VnSensor::readSynchronizationStatus() +{ + char toSend[17]; + + size_t length = Packet::genReadSynchronizationStatus(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + SynchronizationStatusRegister reg; + response.parseSynchronizationStatus( + ®.syncInCount, + ®.syncInTime, + ®.syncOutCount); + + return reg; +} + +void VnSensor::writeSynchronizationStatus(SynchronizationStatusRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteSynchronizationStatus(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.syncInCount, fields.syncInTime, fields.syncOutCount); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeSynchronizationStatus( + const uint32_t &syncInCount, + const uint32_t &syncInTime, + const uint32_t &syncOutCount, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteSynchronizationStatus( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + syncInCount, + syncInTime, + syncOutCount); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +FilterBasicControlRegister VnSensor::readFilterBasicControl() +{ + char toSend[17]; + + size_t length = Packet::genReadFilterBasicControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + FilterBasicControlRegister reg; + uint8_t magMode; + uint8_t extMagMode; + uint8_t extAccMode; + uint8_t extGyroMode; + response.parseFilterBasicControl( + &magMode, + &extMagMode, + &extAccMode, + &extGyroMode, + ®.gyroLimit); + + reg.magMode = static_cast(magMode); + reg.extMagMode = static_cast(extMagMode); + reg.extAccMode = static_cast(extAccMode); + reg.extGyroMode = static_cast(extGyroMode); + + return reg; +} + +void VnSensor::writeFilterBasicControl(FilterBasicControlRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteFilterBasicControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.magMode, fields.extMagMode, fields.extAccMode, fields.extGyroMode, fields.gyroLimit); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeFilterBasicControl( + MagneticMode magMode, + ExternalSensorMode extMagMode, + ExternalSensorMode extAccMode, + ExternalSensorMode extGyroMode, + const vec3f &gyroLimit, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteFilterBasicControl( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + magMode, + extMagMode, + extAccMode, + extGyroMode, + gyroLimit); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +VpeBasicControlRegister VnSensor::readVpeBasicControl() +{ + char toSend[17]; + + size_t length = Packet::genReadVpeBasicControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + VpeBasicControlRegister reg; + uint8_t enable; + uint8_t headingMode; + uint8_t filteringMode; + uint8_t tuningMode; + response.parseVpeBasicControl( + &enable, + &headingMode, + &filteringMode, + &tuningMode); + + reg.enable = static_cast(enable); + reg.headingMode = static_cast(headingMode); + reg.filteringMode = static_cast(filteringMode); + reg.tuningMode = static_cast(tuningMode); + + return reg; +} + +void VnSensor::writeVpeBasicControl(VpeBasicControlRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteVpeBasicControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.enable, fields.headingMode, fields.filteringMode, fields.tuningMode); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeVpeBasicControl( + VpeEnable enable, + HeadingMode headingMode, + VpeMode filteringMode, + VpeMode tuningMode, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteVpeBasicControl( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + enable, + headingMode, + filteringMode, + tuningMode); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +VpeMagnetometerBasicTuningRegister VnSensor::readVpeMagnetometerBasicTuning() +{ + char toSend[17]; + + size_t length = Packet::genReadVpeMagnetometerBasicTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + VpeMagnetometerBasicTuningRegister reg; + response.parseVpeMagnetometerBasicTuning( + ®.baseTuning, + ®.adaptiveTuning, + ®.adaptiveFiltering); + + return reg; +} + +void VnSensor::writeVpeMagnetometerBasicTuning(VpeMagnetometerBasicTuningRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteVpeMagnetometerBasicTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.baseTuning, fields.adaptiveTuning, fields.adaptiveFiltering); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeVpeMagnetometerBasicTuning( + const vec3f &baseTuning, + const vec3f &adaptiveTuning, + const vec3f &adaptiveFiltering, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteVpeMagnetometerBasicTuning( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + baseTuning, + adaptiveTuning, + adaptiveFiltering); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +VpeMagnetometerAdvancedTuningRegister VnSensor::readVpeMagnetometerAdvancedTuning() +{ + char toSend[17]; + + size_t length = Packet::genReadVpeMagnetometerAdvancedTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + VpeMagnetometerAdvancedTuningRegister reg; + response.parseVpeMagnetometerAdvancedTuning( + ®.minFiltering, + ®.maxFiltering, + ®.maxAdaptRate, + ®.disturbanceWindow, + ®.maxTuning); + + return reg; +} + +void VnSensor::writeVpeMagnetometerAdvancedTuning(VpeMagnetometerAdvancedTuningRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteVpeMagnetometerAdvancedTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.minFiltering, fields.maxFiltering, fields.maxAdaptRate, fields.disturbanceWindow, fields.maxTuning); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeVpeMagnetometerAdvancedTuning( + const vec3f &minFiltering, + const vec3f &maxFiltering, + const float &maxAdaptRate, + const float &disturbanceWindow, + const float &maxTuning, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteVpeMagnetometerAdvancedTuning( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + minFiltering, + maxFiltering, + maxAdaptRate, + disturbanceWindow, + maxTuning); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +VpeAccelerometerBasicTuningRegister VnSensor::readVpeAccelerometerBasicTuning() +{ + char toSend[17]; + + size_t length = Packet::genReadVpeAccelerometerBasicTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + VpeAccelerometerBasicTuningRegister reg; + response.parseVpeAccelerometerBasicTuning( + ®.baseTuning, + ®.adaptiveTuning, + ®.adaptiveFiltering); + + return reg; +} + +void VnSensor::writeVpeAccelerometerBasicTuning(VpeAccelerometerBasicTuningRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteVpeAccelerometerBasicTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.baseTuning, fields.adaptiveTuning, fields.adaptiveFiltering); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeVpeAccelerometerBasicTuning( + const vec3f &baseTuning, + const vec3f &adaptiveTuning, + const vec3f &adaptiveFiltering, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteVpeAccelerometerBasicTuning( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + baseTuning, + adaptiveTuning, + adaptiveFiltering); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +VpeAccelerometerAdvancedTuningRegister VnSensor::readVpeAccelerometerAdvancedTuning() +{ + char toSend[17]; + + size_t length = Packet::genReadVpeAccelerometerAdvancedTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + VpeAccelerometerAdvancedTuningRegister reg; + response.parseVpeAccelerometerAdvancedTuning( + ®.minFiltering, + ®.maxFiltering, + ®.maxAdaptRate, + ®.disturbanceWindow, + ®.maxTuning); + + return reg; +} + +void VnSensor::writeVpeAccelerometerAdvancedTuning(VpeAccelerometerAdvancedTuningRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteVpeAccelerometerAdvancedTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.minFiltering, fields.maxFiltering, fields.maxAdaptRate, fields.disturbanceWindow, fields.maxTuning); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeVpeAccelerometerAdvancedTuning( + const vec3f &minFiltering, + const vec3f &maxFiltering, + const float &maxAdaptRate, + const float &disturbanceWindow, + const float &maxTuning, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteVpeAccelerometerAdvancedTuning( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + minFiltering, + maxFiltering, + maxAdaptRate, + disturbanceWindow, + maxTuning); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +VpeGryoBasicTuningRegister VnSensor::readVpeGryoBasicTuning() +{ + char toSend[17]; + + size_t length = Packet::genReadVpeGryoBasicTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + VpeGryoBasicTuningRegister reg; + response.parseVpeGryoBasicTuning( + ®.angularWalkVariance, + ®.baseTuning, + ®.adaptiveTuning); + + return reg; +} + +void VnSensor::writeVpeGryoBasicTuning(VpeGryoBasicTuningRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteVpeGryoBasicTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.angularWalkVariance, fields.baseTuning, fields.adaptiveTuning); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeVpeGryoBasicTuning( + const vec3f &angularWalkVariance, + const vec3f &baseTuning, + const vec3f &adaptiveTuning, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteVpeGryoBasicTuning( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + angularWalkVariance, + baseTuning, + adaptiveTuning); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +vec3f VnSensor::readFilterStartupGyroBias() +{ + char toSend[17]; + + size_t length = Packet::genReadFilterStartupGyroBias(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + vec3f reg; + response.parseFilterStartupGyroBias(®); + + return reg; +} + +void VnSensor::writeFilterStartupGyroBias(const vec3f &bias, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteFilterStartupGyroBias(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), bias); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +MagnetometerCalibrationControlRegister VnSensor::readMagnetometerCalibrationControl() +{ + char toSend[17]; + + size_t length = Packet::genReadMagnetometerCalibrationControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + MagnetometerCalibrationControlRegister reg; + uint8_t hsiMode; + uint8_t hsiOutput; + response.parseMagnetometerCalibrationControl( + &hsiMode, + &hsiOutput, + ®.convergeRate); + + reg.hsiMode = static_cast(hsiMode); + reg.hsiOutput = static_cast(hsiOutput); + + return reg; +} + +void VnSensor::writeMagnetometerCalibrationControl(MagnetometerCalibrationControlRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteMagnetometerCalibrationControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.hsiMode, fields.hsiOutput, fields.convergeRate); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeMagnetometerCalibrationControl( + HsiMode hsiMode, + HsiOutput hsiOutput, + const uint8_t &convergeRate, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteMagnetometerCalibrationControl( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + hsiMode, + hsiOutput, + convergeRate); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +CalculatedMagnetometerCalibrationRegister VnSensor::readCalculatedMagnetometerCalibration() +{ + char toSend[17]; + + size_t length = Packet::genReadCalculatedMagnetometerCalibration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + CalculatedMagnetometerCalibrationRegister reg; + response.parseCalculatedMagnetometerCalibration( + ®.c, + ®.b); + + return reg; +} + +float VnSensor::readIndoorHeadingModeControl() +{ + char toSend[17]; + + size_t length = Packet::genReadIndoorHeadingModeControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + float reg; + response.parseIndoorHeadingModeControl(®); + + return reg; +} + +void VnSensor::writeIndoorHeadingModeControl(const float &maxRateError, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteIndoorHeadingModeControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), maxRateError); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +vec3f VnSensor::readVelocityCompensationMeasurement() +{ + char toSend[17]; + + size_t length = Packet::genReadVelocityCompensationMeasurement(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + vec3f reg; + response.parseVelocityCompensationMeasurement(®); + + return reg; +} + +void VnSensor::writeVelocityCompensationMeasurement(const vec3f &velocity, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteVelocityCompensationMeasurement(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), velocity); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +VelocityCompensationControlRegister VnSensor::readVelocityCompensationControl() +{ + char toSend[17]; + + size_t length = Packet::genReadVelocityCompensationControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + VelocityCompensationControlRegister reg; + uint8_t mode; + response.parseVelocityCompensationControl( + &mode, + ®.velocityTuning, + ®.rateTuning); + + reg.mode = static_cast(mode); + + return reg; +} + +void VnSensor::writeVelocityCompensationControl(VelocityCompensationControlRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteVelocityCompensationControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.mode, fields.velocityTuning, fields.rateTuning); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeVelocityCompensationControl( + VelocityCompensationMode mode, + const float &velocityTuning, + const float &rateTuning, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteVelocityCompensationControl( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + mode, + velocityTuning, + rateTuning); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +VelocityCompensationStatusRegister VnSensor::readVelocityCompensationStatus() +{ + char toSend[17]; + + size_t length = Packet::genReadVelocityCompensationStatus(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + VelocityCompensationStatusRegister reg; + response.parseVelocityCompensationStatus( + ®.x, + ®.xDot, + ®.accelOffset, + ®.omega); + + return reg; +} + +ImuMeasurementsRegister VnSensor::readImuMeasurements() +{ + char toSend[17]; + + size_t length = Packet::genReadImuMeasurements(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + ImuMeasurementsRegister reg; + response.parseImuMeasurements( + ®.mag, + ®.accel, + ®.gyro, + ®.temp, + ®.pressure); + + return reg; +} + +GpsConfigurationRegister VnSensor::readGpsConfiguration() +{ + char toSend[17]; + + size_t length = Packet::genReadGpsConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + GpsConfigurationRegister reg; + uint8_t mode; + uint8_t ppsSource; + response.parseGpsConfiguration( + &mode, + &ppsSource); + + reg.mode = static_cast(mode); + reg.ppsSource = static_cast(ppsSource); + + return reg; +} + +void VnSensor::writeGpsConfiguration(GpsConfigurationRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteGpsConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.mode, fields.ppsSource); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeGpsConfiguration( + GpsMode mode, + PpsSource ppsSource, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteGpsConfiguration( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + mode, + ppsSource); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +vec3f VnSensor::readGpsAntennaOffset() +{ + char toSend[17]; + + size_t length = Packet::genReadGpsAntennaOffset(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + vec3f reg; + response.parseGpsAntennaOffset(®); + + return reg; +} + +void VnSensor::writeGpsAntennaOffset(const vec3f &position, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteGpsAntennaOffset(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), position); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +GpsSolutionLlaRegister VnSensor::readGpsSolutionLla() +{ + char toSend[17]; + + size_t length = Packet::genReadGpsSolutionLla(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + GpsSolutionLlaRegister reg; + uint8_t gpsFix; + response.parseGpsSolutionLla( + ®.time, + ®.week, + &gpsFix, + ®.numSats, + ®.lla, + ®.nedVel, + ®.nedAcc, + ®.speedAcc, + ®.timeAcc); + + reg.gpsFix = static_cast(gpsFix); + + return reg; +} + +GpsSolutionEcefRegister VnSensor::readGpsSolutionEcef() +{ + char toSend[17]; + + size_t length = Packet::genReadGpsSolutionEcef(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + GpsSolutionEcefRegister reg; + uint8_t gpsFix; + response.parseGpsSolutionEcef( + ®.tow, + ®.week, + &gpsFix, + ®.numSats, + ®.position, + ®.velocity, + ®.posAcc, + ®.speedAcc, + ®.timeAcc); + + reg.gpsFix = static_cast(gpsFix); + + return reg; +} + +InsSolutionLlaRegister VnSensor::readInsSolutionLla() +{ + char toSend[17]; + + size_t length = Packet::genReadInsSolutionLla(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + InsSolutionLlaRegister reg; + response.parseInsSolutionLla( + ®.time, + ®.week, + ®.status, + ®.yawPitchRoll, + ®.position, + ®.nedVel, + ®.attUncertainty, + ®.posUncertainty, + ®.velUncertainty); + + return reg; +} + +InsSolutionEcefRegister VnSensor::readInsSolutionEcef() +{ + char toSend[17]; + + size_t length = Packet::genReadInsSolutionEcef(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + InsSolutionEcefRegister reg; + response.parseInsSolutionEcef( + ®.time, + ®.week, + ®.status, + ®.yawPitchRoll, + ®.position, + ®.velocity, + ®.attUncertainty, + ®.posUncertainty, + ®.velUncertainty); + + return reg; +} + +InsAdvancedConfigurationRegister VnSensor::readInsAdvancedConfiguration() +{ + char toSend[17]; + + size_t length = Packet::genReadInsAdvancedConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + InsAdvancedConfigurationRegister reg; + uint8_t useMag; + uint8_t usePres; + uint8_t posAtt; + uint8_t velAtt; + uint8_t velBias; + uint8_t useFoam; + response.parseInsAdvancedConfiguration( + &useMag, + &usePres, + &posAtt, + &velAtt, + &velBias, + &useFoam, + ®.gpsCovType, + ®.velCount, + ®.velInit, + ®.moveOrigin, + ®.gpsTimeout, + ®.deltaLimitPos, + ®.deltaLimitVel, + ®.minPosUncertainty, + ®.minVelUncertainty); + + reg.useMag = useMag != 0; + reg.usePres = usePres != 0; + reg.posAtt = posAtt != 0; + reg.velAtt = velAtt != 0; + reg.velBias = velBias != 0; + reg.useFoam = static_cast(useFoam); + + return reg; +} + +void VnSensor::writeInsAdvancedConfiguration(InsAdvancedConfigurationRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteInsAdvancedConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.useMag, fields.usePres, fields.posAtt, fields.velAtt, fields.velBias, fields.useFoam, fields.gpsCovType, fields.velCount, fields.velInit, fields.moveOrigin, fields.gpsTimeout, fields.deltaLimitPos, fields.deltaLimitVel, fields.minPosUncertainty, fields.minVelUncertainty); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeInsAdvancedConfiguration( + const uint8_t &useMag, + const uint8_t &usePres, + const uint8_t &posAtt, + const uint8_t &velAtt, + const uint8_t &velBias, + FoamInit useFoam, + const uint8_t &gpsCovType, + const uint8_t &velCount, + const float &velInit, + const float &moveOrigin, + const float &gpsTimeout, + const float &deltaLimitPos, + const float &deltaLimitVel, + const float &minPosUncertainty, + const float &minVelUncertainty, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteInsAdvancedConfiguration( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + useMag, + usePres, + posAtt, + velAtt, + velBias, + useFoam, + gpsCovType, + velCount, + velInit, + moveOrigin, + gpsTimeout, + deltaLimitPos, + deltaLimitVel, + minPosUncertainty, + minVelUncertainty); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +InsStateLlaRegister VnSensor::readInsStateLla() +{ + char toSend[17]; + + size_t length = Packet::genReadInsStateLla(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + InsStateLlaRegister reg; + response.parseInsStateLla( + ®.yawPitchRoll, + ®.position, + ®.velocity, + ®.accel, + ®.angularRate); + + return reg; +} + +InsStateEcefRegister VnSensor::readInsStateEcef() +{ + char toSend[17]; + + size_t length = Packet::genReadInsStateEcef(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + InsStateEcefRegister reg; + response.parseInsStateEcef( + ®.yawPitchRoll, + ®.position, + ®.velocity, + ®.accel, + ®.angularRate); + + return reg; +} + +StartupFilterBiasEstimateRegister VnSensor::readStartupFilterBiasEstimate() +{ + char toSend[17]; + + size_t length = Packet::genReadStartupFilterBiasEstimate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + StartupFilterBiasEstimateRegister reg; + response.parseStartupFilterBiasEstimate( + ®.gyroBias, + ®.accelBias, + ®.pressureBias); + + return reg; +} + +void VnSensor::writeStartupFilterBiasEstimate(StartupFilterBiasEstimateRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteStartupFilterBiasEstimate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.gyroBias, fields.accelBias, fields.pressureBias); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeStartupFilterBiasEstimate( + const vec3f &gyroBias, + const vec3f &accelBias, + const float &pressureBias, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteStartupFilterBiasEstimate( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + gyroBias, + accelBias, + pressureBias); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +DeltaThetaAndDeltaVelocityRegister VnSensor::readDeltaThetaAndDeltaVelocity() +{ + char toSend[17]; + + size_t length = Packet::genReadDeltaThetaAndDeltaVelocity(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + DeltaThetaAndDeltaVelocityRegister reg; + response.parseDeltaThetaAndDeltaVelocity( + ®.deltaTime, + ®.deltaTheta, + ®.deltaVelocity); + + return reg; +} + +DeltaThetaAndDeltaVelocityConfigurationRegister VnSensor::readDeltaThetaAndDeltaVelocityConfiguration() +{ + char toSend[17]; + + size_t length = Packet::genReadDeltaThetaAndDeltaVelocityConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + DeltaThetaAndDeltaVelocityConfigurationRegister reg; + uint8_t integrationFrame; + uint8_t gyroCompensation; + uint8_t accelCompensation; + response.parseDeltaThetaAndDeltaVelocityConfiguration( + &integrationFrame, + &gyroCompensation, + &accelCompensation); + + reg.integrationFrame = static_cast(integrationFrame); + reg.gyroCompensation = static_cast(gyroCompensation); + reg.accelCompensation = static_cast(accelCompensation); + + return reg; +} + +void VnSensor::writeDeltaThetaAndDeltaVelocityConfiguration(DeltaThetaAndDeltaVelocityConfigurationRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteDeltaThetaAndDeltaVelocityConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.integrationFrame, fields.gyroCompensation, fields.accelCompensation); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeDeltaThetaAndDeltaVelocityConfiguration( + IntegrationFrame integrationFrame, + CompensationMode gyroCompensation, + CompensationMode accelCompensation, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteDeltaThetaAndDeltaVelocityConfiguration( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + integrationFrame, + gyroCompensation, + accelCompensation); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +ReferenceVectorConfigurationRegister VnSensor::readReferenceVectorConfiguration() +{ + char toSend[17]; + + size_t length = Packet::genReadReferenceVectorConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + ReferenceVectorConfigurationRegister reg; + uint8_t useMagModel; + uint8_t useGravityModel; + response.parseReferenceVectorConfiguration( + &useMagModel, + &useGravityModel, + ®.recalcThreshold, + ®.year, + ®.position); + + reg.useMagModel = useMagModel != 0; + reg.useGravityModel = useGravityModel != 0; + + return reg; +} + +void VnSensor::writeReferenceVectorConfiguration(ReferenceVectorConfigurationRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteReferenceVectorConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.useMagModel, fields.useGravityModel, fields.recalcThreshold, fields.year, fields.position); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeReferenceVectorConfiguration( + const uint8_t &useMagModel, + const uint8_t &useGravityModel, + const uint32_t &recalcThreshold, + const float &year, + const vec3d &position, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteReferenceVectorConfiguration( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + useMagModel, + useGravityModel, + recalcThreshold, + year, + position); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +GyroCompensationRegister VnSensor::readGyroCompensation() +{ + char toSend[17]; + + size_t length = Packet::genReadGyroCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + GyroCompensationRegister reg; + response.parseGyroCompensation( + ®.c, + ®.b); + + return reg; +} + +void VnSensor::writeGyroCompensation(GyroCompensationRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteGyroCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.c, fields.b); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeGyroCompensation( + const mat3f &c, + const vec3f &b, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteGyroCompensation( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + c, + b); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +ImuFilteringConfigurationRegister VnSensor::readImuFilteringConfiguration() +{ + char toSend[17]; + + size_t length = Packet::genReadImuFilteringConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + ImuFilteringConfigurationRegister reg; + uint8_t magFilterMode; + uint8_t accelFilterMode; + uint8_t gyroFilterMode; + uint8_t tempFilterMode; + uint8_t presFilterMode; + response.parseImuFilteringConfiguration( + ®.magWindowSize, + ®.accelWindowSize, + ®.gyroWindowSize, + ®.tempWindowSize, + ®.presWindowSize, + &magFilterMode, + &accelFilterMode, + &gyroFilterMode, + &tempFilterMode, + &presFilterMode); + + reg.magFilterMode = static_cast(magFilterMode); + reg.accelFilterMode = static_cast(accelFilterMode); + reg.gyroFilterMode = static_cast(gyroFilterMode); + reg.tempFilterMode = static_cast(tempFilterMode); + reg.presFilterMode = static_cast(presFilterMode); + + return reg; +} + +void VnSensor::writeImuFilteringConfiguration(ImuFilteringConfigurationRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteImuFilteringConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.magWindowSize, fields.accelWindowSize, fields.gyroWindowSize, fields.tempWindowSize, fields.presWindowSize, fields.magFilterMode, fields.accelFilterMode, fields.gyroFilterMode, fields.tempFilterMode, fields.presFilterMode); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeImuFilteringConfiguration( + const uint16_t &magWindowSize, + const uint16_t &accelWindowSize, + const uint16_t &gyroWindowSize, + const uint16_t &tempWindowSize, + const uint16_t &presWindowSize, + FilterMode magFilterMode, + FilterMode accelFilterMode, + FilterMode gyroFilterMode, + FilterMode tempFilterMode, + FilterMode presFilterMode, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteImuFilteringConfiguration( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + magWindowSize, + accelWindowSize, + gyroWindowSize, + tempWindowSize, + presWindowSize, + magFilterMode, + accelFilterMode, + gyroFilterMode, + tempFilterMode, + presFilterMode); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +GpsCompassBaselineRegister VnSensor::readGpsCompassBaseline() +{ + char toSend[17]; + + size_t length = Packet::genReadGpsCompassBaseline(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + GpsCompassBaselineRegister reg; + response.parseGpsCompassBaseline( + ®.position, + ®.uncertainty); + + return reg; +} + +void VnSensor::writeGpsCompassBaseline(GpsCompassBaselineRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteGpsCompassBaseline(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.position, fields.uncertainty); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeGpsCompassBaseline( + const vec3f &position, + const vec3f &uncertainty, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteGpsCompassBaseline( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + position, + uncertainty); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +GpsCompassEstimatedBaselineRegister VnSensor::readGpsCompassEstimatedBaseline() +{ + char toSend[17]; + + size_t length = Packet::genReadGpsCompassEstimatedBaseline(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + GpsCompassEstimatedBaselineRegister reg; + uint8_t estBaselineUsed; + response.parseGpsCompassEstimatedBaseline( + &estBaselineUsed, + ®.numMeas, + ®.position, + ®.uncertainty); + + reg.estBaselineUsed = estBaselineUsed != 0; + + return reg; +} + +ImuRateConfigurationRegister VnSensor::readImuRateConfiguration() +{ + char toSend[17]; + + size_t length = Packet::genReadImuRateConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + ImuRateConfigurationRegister reg; + response.parseImuRateConfiguration( + ®.imuRate, + ®.navDivisor, + ®.filterTargetRate, + ®.filterMinRate); + + return reg; +} + +void VnSensor::writeImuRateConfiguration(ImuRateConfigurationRegister &fields, bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteImuRateConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.imuRate, fields.navDivisor, fields.filterTargetRate, fields.filterMinRate); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +void VnSensor::writeImuRateConfiguration( + const uint16_t &imuRate, + const uint16_t &navDivisor, + const float &filterTargetRate, + const float &filterMinRate, + bool waitForReply) +{ + char toSend[256]; + + size_t length = Packet::genWriteImuRateConfiguration( + _pi->_sendErrorDetectionMode, + toSend, + sizeof(toSend), + imuRate, + navDivisor, + filterTargetRate, + filterMinRate); + + Packet response; + _pi->transactionNoFinalize(toSend, length, waitForReply, &response); +} + +YawPitchRollTrueBodyAccelerationAndAngularRatesRegister VnSensor::readYawPitchRollTrueBodyAccelerationAndAngularRates() +{ + char toSend[17]; + + size_t length = Packet::genReadYawPitchRollTrueBodyAccelerationAndAngularRates(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + YawPitchRollTrueBodyAccelerationAndAngularRatesRegister reg; + response.parseYawPitchRollTrueBodyAccelerationAndAngularRates( + ®.yawPitchRoll, + ®.bodyAccel, + ®.gyro); + + return reg; +} + +YawPitchRollTrueInertialAccelerationAndAngularRatesRegister VnSensor::readYawPitchRollTrueInertialAccelerationAndAngularRates() +{ + char toSend[17]; + + size_t length = Packet::genReadYawPitchRollTrueInertialAccelerationAndAngularRates(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend)); + + Packet response; + _pi->transactionNoFinalize(toSend, length, true, &response); + + YawPitchRollTrueInertialAccelerationAndAngularRatesRegister reg; + response.parseYawPitchRollTrueInertialAccelerationAndAngularRates( + ®.yawPitchRoll, + ®.inertialAccel, + ®.gyro); + + return reg; +} + +#if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 + +void VnSensor::stopRequest() +{ + _pi->port->stopThread(); +} + +bool VnSensor::threadStopped() +{ + return _pi->port->threadStopped(); +} + +void VnSensor::unregisterListners() +{ + _pi->port->unregisterDataReceivedHandler(); +} + +void VnSensor::shutdownRequest() +{ + if (_pi->DidWeOpenSimplePort) + { + _pi->port->close(); + } + + _pi->DidWeOpenSimplePort = false; + + if (_pi->SimplePortIsOurs) + { + delete _pi->port; + + _pi->port = NULL; + } + +} + +void VnSensor::goRequest() +{ + _pi->port->resumeThread(); +} + +#endif + +} +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/util/dllvalidator.cpp b/ext/vnproglib-1.1.0.115/src/vn/util/dllvalidator.cpp new file mode 100755 index 0000000..7a1b0af --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/util/dllvalidator.cpp @@ -0,0 +1,125 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#if defined WIN32 + +#include "vn/util/dllvalidator.h" + +#include + +void dumpImportDirectory32(PeLib::PeFile32& pef, std::vector& dllNamesOut) +{ + if (pef.readImportDirectory() == PeLib::ERROR_NO_ERROR) + { + const PeLib::ImportDirectory32& imp = static_cast(pef).impDir(); + + for (unsigned int i = 0; i < imp.getNumberOfFiles(PeLib::OLDDIR); i++) + { + dllNamesOut.push_back(imp.getFileName(i, PeLib::OLDDIR)); + } + } +} + +void dumpImportDirectory64(PeLib::PeFile64& pef, std::vector& dllNamesOut) +{ + if (pef.readImportDirectory() == PeLib::ERROR_NO_ERROR) + { + const PeLib::ImportDirectory64& imp = static_cast(pef).impDir(); + + for (unsigned int i = 0; i < imp.getNumberOfFiles(PeLib::OLDDIR); i++) + { + dllNamesOut.push_back(imp.getFileName(i, PeLib::OLDDIR)); + } + } +} + +void DllValidator::DllValidatorVisitor::callback(PeLib::PeFile32 &file) +{ + dumpImportDirectory32(file, mRequiredDlls); +} + +void DllValidator::DllValidatorVisitor::callback(PeLib::PeFile64 &file) +{ + dumpImportDirectory64(file, mRequiredDlls); +} + +DllValidator::DllValidator() +{ +} + +DllValidator::DllValidator(std::string dllName, std::string currentDirectory) : + mIsInitialized(false), + mIsValid(false), + mPeFile(NULL), + mFileName(dllName), + mWorkingDirectory(currentDirectory) +{ +} + +bool DllValidator::initialize() +{ + mPeFile = PeLib::openPeFile(mFileName); + + if (mPeFile != NULL) + { + if ((mPeFile->readMzHeader() == 0) && + (mPeFile->readPeHeader() == 0)) + { + mIsInitialized = true; + } + } + + return mIsInitialized; +} + +bool DllValidator::hasDllNames() +{ + return mVisitor.mRequiredDlls.size() > 0; +} + +void DllValidator::getDllNames(std::vector& dllNamesOut) +{ + dllNamesOut.clear(); + dllNamesOut = mVisitor.mRequiredDlls; +} + +void DllValidator::getMissingDllNames(std::vector& missingDllNamesOut) +{ + missingDllNamesOut.clear(); + missingDllNamesOut = mMissingDlls; +} + +bool DllValidator::validate() +{ + mIsValid = false; + + if (mIsInitialized) + { + SetCurrentDirectory(mWorkingDirectory.c_str()); + + mMissingDlls.clear(); + + mPeFile->visit(mVisitor); + + for (size_t index = 0; index < mVisitor.mRequiredDlls.size(); index++) + { + HMODULE module = LoadLibraryEx(mVisitor.mRequiredDlls[index].c_str(), NULL, LOAD_LIBRARY_AS_IMAGE_RESOURCE); + if (NULL == module) + { + mMissingDlls.push_back(mVisitor.mRequiredDlls[index]); + } + } + } + else if (mPeFile == NULL) + { + mMissingDlls.push_back(mFileName + " is not a valid DLL file."); + } + + if (mMissingDlls.empty()) + { + mIsValid = true; + } + + return mIsValid; +} + +#endif diff --git a/ext/vnproglib-1.1.0.115/src/vn/util/memoryport.cpp b/ext/vnproglib-1.1.0.115/src/vn/util/memoryport.cpp new file mode 100755 index 0000000..7603a7c --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/util/memoryport.cpp @@ -0,0 +1,190 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +/// {COMMON_HEADER} +#include "vn/util/memoryport.h" + +#include + +#include "vn/xplat/criticalsection.h" +#include "vn/exceptions.h" + +using namespace std; +using namespace vn::xplat; + +namespace vn { +namespace util { + +struct MemoryPort::Impl +{ + // Indiates if the serial port is open. + bool IsOpen; + + // Critical section for registering, unregistering, and notifying observers + // of events. + CriticalSection ObserversCriticalSection; + + DataReceivedHandler _dataReceivedHandler; + void* _dataReceivedUserData; + DataWrittenHandler _dataWrittenHandler; + void* _dataWrittenUserData; + + const uint8_t* DataAvailableForRead; + + size_t DataAvailableForReadLength; + + MemoryPort *BackReference; + + explicit Impl(MemoryPort* backReference) : + IsOpen(false), + _dataReceivedHandler(NULL), + _dataReceivedUserData(NULL), + _dataWrittenHandler(NULL), + _dataWrittenUserData(NULL), + DataAvailableForRead(NULL), + DataAvailableForReadLength(0), + BackReference(backReference) + { } + + void OnDataReceived() + { + if (_dataReceivedHandler == NULL) + return; + + ObserversCriticalSection.enter(); + + _dataReceivedHandler(_dataReceivedUserData); + + ObserversCriticalSection.leave(); + } +}; + +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable:4355) +#endif + +MemoryPort::MemoryPort() : + _pi(new Impl(this)) +{ +} + +#if defined (_MSC_VER) + #pragma warning(pop) +#endif + +MemoryPort::~MemoryPort() +{ + delete _pi; +} + +void MemoryPort::open() +{ + if (_pi->IsOpen) + throw invalid_operation(); + + _pi->IsOpen = true; +} + +void MemoryPort::close() +{ + if (!_pi->IsOpen) + throw invalid_operation(); + + _pi->IsOpen = false; +} + +bool MemoryPort::isOpen() +{ + return _pi->IsOpen; +} + +void MemoryPort::write(const char data[], size_t length) +{ + if (!_pi->IsOpen) + throw invalid_operation(); + + if (_pi->_dataWrittenHandler != NULL) + _pi->_dataWrittenHandler(_pi->_dataWrittenUserData, data, length); +} + +void MemoryPort::read(char dataBuffer[], size_t numOfBytesToRead, size_t &numOfBytesActuallyRead) +{ + if (!_pi->IsOpen) + throw invalid_operation(); + + if (numOfBytesToRead < _pi->DataAvailableForReadLength) + // Don't support less than full read for now. + throw not_implemented(); + + copy(_pi->DataAvailableForRead, _pi->DataAvailableForRead + _pi->DataAvailableForReadLength, dataBuffer); + + numOfBytesActuallyRead = _pi->DataAvailableForReadLength; +} + +void MemoryPort::registerDataReceivedHandler(void* userData, DataReceivedHandler handler) +{ + if (_pi->_dataReceivedHandler != NULL) + throw invalid_operation(); + + _pi->ObserversCriticalSection.enter(); + + _pi->_dataReceivedHandler = handler; + _pi->_dataReceivedUserData = userData; + + _pi->ObserversCriticalSection.leave(); +} + +void MemoryPort::unregisterDataReceivedHandler() +{ + if (_pi->_dataReceivedHandler == NULL) + throw invalid_operation(); + + _pi->ObserversCriticalSection.enter(); + + _pi->_dataReceivedHandler = NULL; + _pi->_dataReceivedUserData = NULL; + + _pi->ObserversCriticalSection.leave(); +} + +void MemoryPort::registerDataWrittenHandler(void* userData, DataWrittenHandler handler) +{ + if (_pi->_dataWrittenHandler != NULL) + throw invalid_operation(); + + _pi->_dataWrittenHandler = handler; + _pi->_dataWrittenUserData = userData; +} + +void MemoryPort::unregisterDataWrittenHandler() +{ + if (_pi->_dataWrittenHandler == NULL) + throw invalid_operation(); + + _pi->_dataWrittenHandler = NULL; + _pi->_dataWrittenUserData = NULL; +} + +void MemoryPort::SendDataBackDoor(const uint8_t data[], size_t length) +{ + _pi->DataAvailableForRead = data; + _pi->DataAvailableForReadLength = length; + + _pi->OnDataReceived(); +} + +void MemoryPort::SendDataBackDoor( + const char data[], + std::size_t length) +{ + SendDataBackDoor(reinterpret_cast(data), length); +} + +void MemoryPort::SendDataBackDoor( + const std::string data) +{ + SendDataBackDoor(data.c_str(), data.length()); +} + +} +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/utilities.cpp b/ext/vnproglib-1.1.0.115/src/vn/utilities.cpp new file mode 100755 index 0000000..7430812 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/utilities.cpp @@ -0,0 +1,205 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/util/export.h" + +#if defined _WINDOWS && PYTHON + // TODO : This needs to function in Linux as well + #include "vn/util/dllvalidator.h" +#else + #if defined __linux__ + #endif +#endif + +#include "vn/utilities.h" + +#include + +#if _M_IX86 || __i386__ || __x86_64 || _WIN64 + // Compiling for x86 processor. + #define VN_LITTLE_ENDIAN 1 +#elif __linux__ + // Don't know what processor we are compiling for but we have endian.h. + #define VN_HAVE_ENDIAN_H 1 + #include +#elif __BYTE_ORDER__ + #if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ + #define VN_LITTLE_ENDIAN 1 + #elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__ + #define VN_BIG_ENDIAN 1 + #else + #error "Unknown System" + #endif +#else + #error "Unknown System" +#endif + +using namespace std; + +namespace vn { + +int ApiVersion::major() +{ + return VNAPI_MAJOR; +} + +int ApiVersion::minor() +{ + return VNAPI_MINOR; +} + +int ApiVersion::patch() +{ + return VNAPI_PATCH; +} + +int ApiVersion::revision() +{ + return VNAPI_REVISION; +} + +string ApiVersion::getVersion() +{ + stringstream ss; + + ss << major() << "." << minor() << "." << patch() << "." << revision(); + + return ss.str(); +} + +uint8_t toUint8FromHexStr(char const* str) +{ + uint8_t result; + + result = to_uint8_from_hexchar(str[0]) << 4; + result += to_uint8_from_hexchar(str[1]); + + return result; +} + +uint16_t stoh(uint16_t sensorOrdered) +{ + #if VN_LITTLE_ENDIAN + return sensorOrdered; + #elif VN_BIG_ENDIAN + uint16_t host; + host = ((sensorOrdered >> 0) & 0xFF) * 0x0100; + host += ((sensorOrdered >> 8) & 0xFF) * 0x0001; + return host; + #elif VN_HAVE_ENDIAN_H + return le16toh(sensorOrdered); + #else + #error("Unknown system") + #endif +} + +uint32_t stoh(uint32_t sensorOrdered) +{ + #if VN_LITTLE_ENDIAN + return sensorOrdered; + #elif VN_BIG_ENDIAN + uint32_t host; + host = ((sensorOrdered >> 0) & 0xFF) * 0x01000000; + host += ((sensorOrdered >> 8) & 0xFF) * 0x00010000; + host += ((sensorOrdered >> 16) & 0xFF) * 0x00000100; + host += ((sensorOrdered >> 24) & 0xFF) * 0x00000001; + return host; + #elif VN_HAVE_ENDIAN_H + return le32toh(sensorOrdered); + #else + #error("Unknown system") + #endif +} + +uint64_t stoh(uint64_t sensorOrdered) +{ + #if VN_LITTLE_ENDIAN + return sensorOrdered; + #elif VN_BIG_ENDIAN + uint64_t host; + host = ((sensorOrdered >> 0) & 0xFF) * 0x0100000000000000; + host += ((sensorOrdered >> 8) & 0xFF) * 0x0001000000000000; + host += ((sensorOrdered >> 16) & 0xFF) * 0x0000010000000000; + host += ((sensorOrdered >> 24) & 0xFF) * 0x0000000100000000; + host += ((sensorOrdered >> 32) & 0xFF) * 0x0000000001000000; + host += ((sensorOrdered >> 40) & 0xFF) * 0x0000000000010000; + host += ((sensorOrdered >> 48) & 0xFF) * 0x0000000000000100; + host += ((sensorOrdered >> 56) & 0xFF) * 0x0000000000000001; + return host; + #elif VN_HAVE_ENDIAN_H + return le64toh(sensorOrdered); + #else + #error("Unknown system") + #endif +} + +uint8_t countSetBits(uint8_t d) +{ + uint8_t count = 0; + + while (d) + { + d &= (d - 1); + count++; + } + + return count; +} + + + +uint8_t to_uint8_from_hexchar(char c) +{ + if (c < ':') + return c - '0'; + + if (c < 'G') + return c - '7'; + + return c - 'W'; +} + +uint8_t to_uint8_from_hexstr(char const* str) +{ + return toUint8FromHexStr(str); +} + +uint16_t to_uint16_from_hexstr(char const* str) +{ + uint16_t result; + + result = to_uint8_from_hexstr(str) << 8; + result += to_uint8_from_hexstr(str + 2); + + return result; +} + +#if PYTHON + +bool checkDllValidity(const std::string& dllName, const std::string& workingDirectory, std::vector& missingDlls) +{ + #if defined _WINDOWS + bool isValid = false; + + missingDlls.clear(); + + DllValidator validator(dllName, workingDirectory); + validator.initialize(); + isValid = validator.validate(); + validator.getMissingDllNames(missingDlls); + + return isValid; + #else + #if defined _LINUX + // TODO : To avoid lots of rework I am simply making this return true if _LINUX is defined + // Once this is implemented for Linux rework will be minimal + return true; + #else + // TODO : Any other systems that need to implement this will go here. + return true; + #endif + #endif +} + +#endif + +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/xplat/criticalsection.cpp b/ext/vnproglib-1.1.0.115/src/vn/xplat/criticalsection.cpp new file mode 100755 index 0000000..2f37713 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/xplat/criticalsection.cpp @@ -0,0 +1,83 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/xplat/criticalsection.h" + +#if _WIN32 + #include +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + #include +#else + #error "Unknown System" +#endif + +#include "vn/exceptions.h" + +using namespace std; + +namespace vn { +namespace xplat { + +struct CriticalSection::Impl +{ + #if _WIN32 + CRITICAL_SECTION CriticalSection; + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + pthread_mutex_t CriticalSection; + #else + #error "Unknown System" + #endif + + Impl() + { + } +}; + +CriticalSection::CriticalSection() : + _pi(new Impl()) +{ + #if _WIN32 + InitializeCriticalSection(&_pi->CriticalSection); + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + pthread_mutex_init(&_pi->CriticalSection, NULL); + #else + #error "Unknown System" + #endif +} + +CriticalSection::~CriticalSection() +{ + #if _WIN32 + DeleteCriticalSection(&_pi->CriticalSection); + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + pthread_mutex_destroy(&_pi->CriticalSection); + #else + #error "Unknown System" + #endif + + delete _pi; +} + +void CriticalSection::enter() +{ + #if _WIN32 + EnterCriticalSection(&_pi->CriticalSection); + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + pthread_mutex_lock(&_pi->CriticalSection); + #else + #error "Unknown System" + #endif +} + +void CriticalSection::leave() +{ + #if _WIN32 + LeaveCriticalSection(&_pi->CriticalSection); + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + pthread_mutex_unlock(&_pi->CriticalSection); + #else + #error "Unknown System" + #endif +} + +} +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/xplat/event.cpp b/ext/vnproglib-1.1.0.115/src/vn/xplat/event.cpp new file mode 100755 index 0000000..3d99a72 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/xplat/event.cpp @@ -0,0 +1,272 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/xplat/event.h" + +#if _WIN32 + #include +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + #include + #include +#else + #error "Unknown System" +#endif + +#if __APPLE__ + #include + #include + #include + #include +#endif + +#include "vn/exceptions.h" + +using namespace std; + +namespace vn { +namespace xplat { + +struct Event::Impl +{ + #if _WIN32 + HANDLE EventHandle; + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + pthread_mutex_t Mutex; + pthread_cond_t Condition; + bool IsTriggered; + #else + #error "Unknown System" + #endif + + Impl() : + #if _WIN32 + EventHandle(NULL) + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + IsTriggered(false) + #else + #error "Unknown System" + #endif + { + #if _WIN32 + + EventHandle = CreateEvent( + NULL, + false, + false, + NULL); + + if (EventHandle == NULL) + throw unknown_error(); + + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + pthread_mutex_init(&Mutex, NULL); + + pthread_cond_init(&Condition, NULL); + + #else + + #error "Unknown System" + + #endif + } +}; + +Event::Event() : + _pi(new Impl()) +{ } + +Event::~Event() +{ + #if _WIN32 + CloseHandle(_pi->EventHandle); + #endif + + delete _pi; +} + +void Event::wait() +{ + #if _WIN32 + + DWORD result; + + result = WaitForSingleObject( + _pi->EventHandle, + INFINITE); + + if (result == WAIT_OBJECT_0) + return; + + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + pthread_mutex_lock(&_pi->Mutex); + + int errorCode = pthread_cond_wait( + &_pi->Condition, + &_pi->Mutex); + + pthread_mutex_unlock(&_pi->Mutex); + + if (errorCode == 0) + return; + + #else + + #error "Unknown System" + + #endif + + throw unknown_error(); +} + +Event::WaitResult Event::waitUs(uint32_t timeoutInMicroSec) +{ + #if _WIN32 + + DWORD result; + + result = WaitForSingleObject( + _pi->EventHandle, + timeoutInMicroSec / 1000); + + if (result == WAIT_OBJECT_0) + return WAIT_SIGNALED; + + if (result == WAIT_TIMEOUT) + return WAIT_TIMEDOUT; + + #elif __linux__ || __CYGWIN__ || __QNXNTO__ + + pthread_mutex_lock(&_pi->Mutex); + + timespec now; + clock_gettime(CLOCK_REALTIME, &now); + + uint32_t numOfSecs = timeoutInMicroSec / 1000000; + uint32_t numOfNanoseconds = (timeoutInMicroSec % 1000000) * 1000; + + now.tv_sec += numOfSecs; + now.tv_nsec += numOfNanoseconds; + + if (now.tv_nsec > 1000000000) + { + now.tv_nsec %= 1000000000; + now.tv_sec++; + } + + int errorCode = pthread_cond_timedwait( + &_pi->Condition, + &_pi->Mutex, + &now); + + pthread_mutex_unlock(&_pi->Mutex); + + if (errorCode == 0) + return WAIT_SIGNALED; + + if (errorCode == ETIMEDOUT) + return WAIT_TIMEDOUT; + + #elif __APPLE__ + + pthread_mutex_lock(&_pi->Mutex); + + clock_serv_t cclock; + mach_timespec_t mts; + host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock); + clock_get_time(cclock, &mts); + mach_port_deallocate(mach_task_self(), cclock); + + timespec now; + now.tv_sec = mts.tv_sec; + now.tv_nsec = mts.tv_nsec; + + uint32_t numOfSecs = timeoutInMicroSec / 1000000; + uint32_t numOfNanoseconds = (timeoutInMicroSec % 1000000) * 1000; + + now.tv_sec += numOfSecs; + now.tv_nsec += numOfNanoseconds; + + if (now.tv_nsec > 1000000000) + { + now.tv_nsec %= 1000000000; + now.tv_sec++; + } + + int errorCode = pthread_cond_timedwait( + &_pi->Condition, + &_pi->Mutex, + &now); + + pthread_mutex_unlock(&_pi->Mutex); + + if (errorCode == 0) + return WAIT_SIGNALED; + + if (errorCode == ETIMEDOUT) + return WAIT_TIMEDOUT; + + #else + + #error "Unknown System" + + #endif + + throw unknown_error(); +} + +Event::WaitResult Event::waitMs(uint32_t timeoutInMs) +{ + #if _WIN32 + + DWORD result; + + result = WaitForSingleObject( + _pi->EventHandle, + timeoutInMs); + + if (result == WAIT_OBJECT_0) + return WAIT_SIGNALED; + + if (result == WAIT_TIMEOUT) + return WAIT_TIMEDOUT; + + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + return waitUs(timeoutInMs * 1000); + + #else + + #error "Unknown System" + + #endif + + throw unknown_error(); +} + +void Event::signal() +{ + #if _WIN32 + + if (!SetEvent(_pi->EventHandle)) + throw unknown_error(); + + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + pthread_mutex_lock(&_pi->Mutex); + + _pi->IsTriggered = true; + + pthread_cond_signal(&_pi->Condition); + + pthread_mutex_unlock(&_pi->Mutex); + + #else + + #error "Unknown System" + + #endif +} + +} +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/xplat/port.cpp b/ext/vnproglib-1.1.0.115/src/vn/xplat/port.cpp new file mode 100755 index 0000000..8489af8 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/xplat/port.cpp @@ -0,0 +1,11 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/xplat/port.h" + +namespace vn { +namespace xplat { + +IPort::~IPort() { } + +} +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/xplat/python.cpp b/ext/vnproglib-1.1.0.115/src/vn/xplat/python.cpp new file mode 100755 index 0000000..b9cbac3 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/xplat/python.cpp @@ -0,0 +1,103 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "boost/python.hpp" +#include "boost/python/scope.hpp" + +#include "vn/xplat/port.h" +#include "vn/xplat/serialport.h" +//#include "vn/event.h" +#include "vn/xplat/thread.h" +#include "vn/python/util.h" +#include "vn/xplat/time.h" + +using namespace boost::python; +using namespace vn; +using namespace vn::xplat; + +void writeWrapper(IPort& self, str out) +{ + std::string toWrite = extract(out); + self.write(toWrite.c_str(), toWrite.size()); +} + +list readWrapper(IPort& self, size_t numBytesToRead) +{ + char* buf = new char[numBytesToRead]; + list r; + size_t numBytesActuallyRead = 0; + + self.read(buf, numBytesToRead, numBytesActuallyRead); + + for (size_t i = 0; i < numBytesActuallyRead; i++) + r.append(buf[i]); + + delete [] buf; + + return r; +} + +list getPortNames() +{ + std::vector names = SerialPort::getPortNames(); + + list pl; + + for (std::vector::iterator it = names.begin(); it != names.end(); ++it) + pl.append(*it); + //for (std::string n : names) + // pl.append(n); + + return pl; +} + +void threadSleepSecWrapper(uint32_t numOfSecsToSleep) +{ + python::ReleaseGIL scopedRelase; + + Thread::sleepSec(numOfSecsToSleep); +} + +void threadSleepMsWrapper(uint32_t numOfMsToSleep) +{ + python::ReleaseGIL scopedRelase; + + Thread::sleepMs(numOfMsToSleep); +} + +BOOST_PYTHON_MODULE(_xplat) +{ +// class_("Event") +// //.def(self += int()) +// .def(self += &PyObject()) +// .def("fire", &Event::fire) +// ; + + class_("TimeStamp", no_init) + .def("get", &TimeStamp::get).staticmethod("get") + .def_readonly("_sec", &TimeStamp::_sec) + .def_readonly("_usec", &TimeStamp::_usec) + ; + + class_("Port", no_init) + .def("open", &IPort::open) + .def("write", &writeWrapper) + .def("read", &readWrapper) + ; + + class_("Thread", no_init) + //.def("sleep_sec", &Thread::sleepSec).staticmethod("sleep_sec") + .def("sleep_sec", &threadSleepSecWrapper).staticmethod("sleep_sec") + .def("sleep_ms", &threadSleepMsWrapper).staticmethod("sleep_ms") + ; + + scope serialPortScope = class_, boost::noncopyable>("SerialPort", "Represents a cross-platform serial port.", init()) + .def("get_port_names", &getPortNames, "The list of available serial port names.") + .add_property("stop_bits", &SerialPort::stopBits, &SerialPort::setStopBits) + ; + + enum_("StopBits") + .value("ONE_STOP_BIT", SerialPort::ONE_STOP_BIT) + .value("TWO_STOP_BITS", SerialPort::TWO_STOP_BITS) + .export_values(); + +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/xplat/serialport.cpp b/ext/vnproglib-1.1.0.115/src/vn/xplat/serialport.cpp new file mode 100755 index 0000000..17f54be --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/xplat/serialport.cpp @@ -0,0 +1,1405 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/xplat/serialport.h" + +#if _WIN32 + #include + #include + #include + #include + #if _UNICODE + #else + #include + #endif +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + #include + #include + #include + #include + #include + #include + #include + #include +#else + #error "Unknown System" +#endif + +#if __linux__ + #include +#elif __APPLE__ + #include +#endif + +#include +#include + +#include "vn/xplat/thread.h" +#include "vn/xplat/criticalsection.h" +#include "vn/exceptions.h" +#include "vn/xplat/event.h" +#include "vn/util/compiler.h" + +#if PYTHON + #include "vn/python/util.h" +#endif + +using namespace std; + +namespace vn { +namespace xplat { + +struct SerialPort::Impl +{ + + // Constants ////////////////////////////////////////////////////////////// + + static const size_t NumberOfBytesToPurgeOnOpeningSerialPort = 100; + + static const uint8_t WaitTimeForSerialPortReadsInMs = 100; + + // Members //////////////////////////////////////////////////////////////// + + #if _WIN32 + HANDLE SerialPortHandle; + size_t NumberOfReceiveDataDroppedSections; + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + int SerialPortHandle; + #else + #error "Unknown System" + #endif + + Thread *pThreadForHandlingReceivedDataInternally; + + // The name of the serial port. + string PortName; + + // The serial port's baudrate. + uint32_t Baudrate; + + // Indicates if the serial port is open. + bool IsOpen; + + // Critical section for registering, unregistering, and notifying observers + // of events. + CriticalSection ObserversCriticalSection; + + DataReceivedHandler _dataReceivedHandler; + void* _dataReceivedUserData; + + Thread *pSerialPortEventsThread; + + bool ContinueHandlingSerialPortEvents; + bool ChangingBaudrate; + + bool PurgeFirstDataBytesWhenSerialPortIsFirstOpened; + + SerialPort* BackReference; + + StopBits stopBits; + + Event WaitForBaudrateChange; + Event NotificationsThreadStopped; + + #if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 + bool ExternalStopRequest; + bool ThreadStopped; + #endif + + explicit Impl(SerialPort* backReference) : + #if _WIN32 + NumberOfReceiveDataDroppedSections(0), + SerialPortHandle(NULL), + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + SerialPortHandle(0), + #else + #error "Unknown System" + #endif + pThreadForHandlingReceivedDataInternally(NULL), + Baudrate(0), + IsOpen(false), + _dataReceivedHandler(NULL), + _dataReceivedUserData(NULL), + pSerialPortEventsThread(NULL), + ContinueHandlingSerialPortEvents(false), + ChangingBaudrate(false), + PurgeFirstDataBytesWhenSerialPortIsFirstOpened(true), + #if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 + ExternalStopRequest(false), + ThreadStopped(false), + #endif + BackReference(backReference), + stopBits(ONE_STOP_BIT) + { } + + static void HandleSerialPortNotifications(void* data) + { + static_cast(data)->HandleSerialPortNotifications(); + } + + void close(bool checkAndToggleIsOpenFlag = true) + { + if (checkAndToggleIsOpenFlag) + ensureOpened(); + + StopSerialPortNotificationsThread(); + + #if _WIN32 + + if (!CloseHandle(SerialPortHandle)) + throw unknown_error(); + + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + if (::close(SerialPortHandle) == -1) + throw unknown_error(); + + #else + #error "Unknown System" + #endif + + if (checkAndToggleIsOpenFlag) + IsOpen = false; + } + + void closeAfterUsbCableUnplugged() + { + #if _WIN32 + + if (!CloseHandle(SerialPortHandle)) + throw unknown_error(); + + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + if (::close(SerialPortHandle) == -1) + throw unknown_error(); + + #else + #error "Unknown System" + #endif + + IsOpen = false; + } + + #if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 + + void externalStop() + { + ExternalStopRequest = true; + } + + void externalStopFinished() + { + ExternalStopRequest = false; + } + + #endif + + void HandleSerialPortNotifications() + { + bool userUnpluggedUsbCable = false; + + #if _WIN32 + + OVERLAPPED overlapped; + + memset(&overlapped, 0, sizeof(OVERLAPPED)); + + overlapped.hEvent = CreateEvent( + NULL, + false, + false, + NULL); + + SetCommMask( + SerialPortHandle, + EV_RXCHAR | EV_ERR | EV_RX80FULL); + + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + fd_set readfs; + int error; + timeval readWaitTime; + + #else + + #error "Unknown System" + + #endif + + IgnoreError: + + try + { + while (ContinueHandlingSerialPortEvents) + { + #if _WIN32 + + #if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 + + if (ExternalStopRequest) + { + ThreadStopped = true; + while (ExternalStopRequest) + { + Sleep(1); + } + ThreadStopped = false; + } + + #endif + + DWORD mask = 0; + DWORD temp = 0; + + BOOL result = WaitCommEvent( + SerialPortHandle, + &mask, + &overlapped); + + if (result) + { + OnDataReceived(); + + continue; + } + + if (GetLastError() != ERROR_IO_PENDING) + // Something unexpected happened. + break; + + KeepWaiting: + + // We need to wait for the event to occur. + DWORD waitResult = WaitForSingleObject( + overlapped.hEvent, + WaitTimeForSerialPortReadsInMs); + + if (!ContinueHandlingSerialPortEvents) + break; + + if (waitResult == WAIT_TIMEOUT) + goto KeepWaiting; + + if (waitResult != WAIT_OBJECT_0) + // Something unexpected happened. + break; + + if (!GetOverlappedResult( + SerialPortHandle, + &overlapped, + &temp, + TRUE)) + { + if (GetLastError() == ERROR_OPERATION_ABORTED) + { + // Possibly the user unplugged the UART-to-USB cable. + ContinueHandlingSerialPortEvents = false; + userUnpluggedUsbCable = true; + } + + // Something unexpected happened. + break; + } + + if (mask & EV_RXCHAR) + { + OnDataReceived(); + + continue; + } + + if (mask & EV_RX80FULL) + { + // We assume the RX buffer was overrun. + NumberOfReceiveDataDroppedSections++; + + continue; + } + + if (mask & EV_ERR) + { + DWORD spErrors; + COMSTAT comStat; + + if (!ClearCommError( + SerialPortHandle, + &spErrors, + &comStat)) + { + // Something unexpected happened. + break; + } + + if ((spErrors & CE_OVERRUN) || (spErrors & CE_RXOVER)) + { + // The serial buffer RX buffer was overrun. + NumberOfReceiveDataDroppedSections++; + } + + continue; + } + + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + FD_SET(SerialPortHandle, &readfs); + + // Select sets the values in readWaitTime. + readWaitTime.tv_sec = 0; + readWaitTime.tv_usec = WaitTimeForSerialPortReadsInMs * 1000; + + error = select( + SerialPortHandle + 1, + &readfs, + NULL, + NULL, + &readWaitTime); + + if (error == -1) + { + #if __CYGWIN__ + + if (errno == EINVAL) + { + // Sometime when running the example getting_started, + // this condition will hit. I assume it is a race + // condition with the operating system (actually this + // problem was noticed running CYGWIN) but appears to + // work when we try it again later. + goto IgnoreError; + } + + #endif + + // Something unexpected happened. + break; + } + + if (!FD_ISSET(SerialPortHandle, &readfs)) + continue; + + OnDataReceived(); + + #else + #error "Unknown System" + #endif + + } + } + catch (...) + { + // Don't want user-code exceptions stopping the thread. + goto IgnoreError; + } + + if (ContinueHandlingSerialPortEvents) + // An error must have occurred. + throw unknown_error(); + + /*if (ChangingBaudrate) + { + NotificationsThreadStopped.signal(); + WaitForBaudrateChange.wait(); + + goto IgnoreError; + }*/ + + #if _WIN32 + + if (!userUnpluggedUsbCable) + { + SetCommMask( + SerialPortHandle, + 0); + } + + #endif + + if (userUnpluggedUsbCable) + closeAfterUsbCableUnplugged(); + } + + void StartSerialPortNotificationsThread() + { + ContinueHandlingSerialPortEvents = true; + + pSerialPortEventsThread = Thread::startNew( + HandleSerialPortNotifications, + this); + } + + void PurgeFirstDataBytesFromSerialPort() + { + char buffer[NumberOfBytesToPurgeOnOpeningSerialPort]; + size_t numOfBytesRead; + + BackReference->read( + buffer, + NumberOfBytesToPurgeOnOpeningSerialPort, + numOfBytesRead); + } + + void StopSerialPortNotificationsThread() + { + ContinueHandlingSerialPortEvents = false; + + #if PYTHON && (PL156_ORIGINAL || PL156_FIX_ATTEMPT_1) + pSerialPortEventsThread->join(); + #endif + + delete pSerialPortEventsThread; + } + + /*void AlertSerialPortNotificationsThreadOfBaudrateChange() + { + ChangingBaudrate = true; + + ContinueHandlingSerialPortEvents = false; + + NotificationsThreadStopped.wait(); + + ContinueHandlingSerialPortEvents = true; + }*/ + + void OnDataReceived() + { + bool exception_happened = false; + exception rethrow; + + ObserversCriticalSection.enter(); + + // This is a critical section block + // The exception must be handled here or the critical section will never + // unlock + try + { + // Moved this NULL check down here due to the nature of critical sections. + // The handler could easily be changed to NULL while waiting for the lock + if (_dataReceivedHandler != NULL) + { + _dataReceivedHandler(_dataReceivedUserData); + } + else + { + return; + } + } + catch (exception e) + { + // We still want to throw an exception if it happens + // Set a flag to indicate we need to throw an exception + exception_happened = true; + rethrow = e; + } + + ObserversCriticalSection.leave(); + + // Rethrow the exception + if (exception_happened) + { + throw rethrow; + } + } + + void ensureOpened() + { + if (!IsOpen) + throw invalid_operation("Port is not opened."); + } + + void ensureClosed() + { + if (IsOpen) + throw invalid_operation("Port is not closed."); + } + + void open(bool checkAndToggleIsOpenFlag = true) + { + if (checkAndToggleIsOpenFlag) + ensureClosed(); + + #if _WIN32 + + DCB config; + COMMTIMEOUTS comTimeOut; + + string fullPortName = "\\\\.\\" + PortName; + + SerialPortHandle = CreateFileA( + fullPortName.c_str(), + GENERIC_READ | GENERIC_WRITE, + 0, + NULL, + OPEN_EXISTING, + FILE_FLAG_OVERLAPPED, + NULL); + + if (SerialPortHandle == INVALID_HANDLE_VALUE) + { + DWORD error = GetLastError(); + + if (error == ERROR_ACCESS_DENIED) + // Port already open, probably. + throw invalid_operation("Port '" + PortName + "' already open."); + + if (error == ERROR_FILE_NOT_FOUND) + // Port probably does not exist. + throw not_found(PortName); + + throw unknown_error(); + } + + // Set the state of the COM port. + if (!GetCommState(SerialPortHandle, &config)) + { + DWORD error = GetLastError(); + + if (error != ERROR_OPERATION_ABORTED) + throw unknown_error(); + + // Try clearing this error. + DWORD errors; + if (!ClearCommError(SerialPortHandle, &errors, NULL)) + throw unknown_error(); + + // Retry the operation. + if (!GetCommState(SerialPortHandle, &config)) + throw unknown_error(); + } + + switch (stopBits) + { + case StopBits::ONE_STOP_BIT: + config.StopBits = ONESTOPBIT; + break; + + case StopBits::TWO_STOP_BITS: + config.StopBits = TWOSTOPBITS; + break; + + default: + throw not_implemented(); + } + + config.BaudRate = Baudrate; + config.Parity = NOPARITY; + config.ByteSize = 8; + config.fAbortOnError = 0; + + if (!SetCommState(SerialPortHandle, &config)) + { + DWORD error = GetLastError(); + + if (error == ERROR_INVALID_PARAMETER) + { + if (!CloseHandle(SerialPortHandle)) + throw unknown_error(); + + throw invalid_argument("Unsupported baudrate."); + } + + if (error != ERROR_OPERATION_ABORTED) + throw unknown_error(); + + // Try clearing this error. + DWORD errors; + if (!ClearCommError(SerialPortHandle, &errors, NULL)) + throw unknown_error(); + + // Retry the operation. + if (!SetCommState(SerialPortHandle, &config)) + throw unknown_error(); + } + + comTimeOut.ReadIntervalTimeout = 0; + comTimeOut.ReadTotalTimeoutMultiplier = 0; + comTimeOut.ReadTotalTimeoutConstant = 1; + comTimeOut.WriteTotalTimeoutMultiplier = 3; + comTimeOut.WriteTotalTimeoutConstant = 2; + + if (!SetCommTimeouts(SerialPortHandle, &comTimeOut)) + { + DWORD error = GetLastError(); + + if (error != ERROR_OPERATION_ABORTED) + throw unknown_error(); + + // Try clearing this error. + DWORD errors; + if (!ClearCommError(SerialPortHandle, &errors, NULL)) + throw unknown_error(); + + // Retry the operation. + if (!SetCommTimeouts(SerialPortHandle, &comTimeOut)) + throw unknown_error(); + } + + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + int portFd = -1; + + portFd = ::open( + PortName.c_str(), + #if __linux__ || __CYGWIN__ || __QNXNTO__ + O_RDWR | O_NOCTTY); + #elif __APPLE__ + O_RDWR | O_NOCTTY | O_NONBLOCK); + #else + #error "Unknown System" + #endif + + if (portFd == -1) + { + switch (errno) + { + case EACCES: + throw permission_denied(PortName); + case ENXIO: + case ENOTDIR: + case ENOENT: + throw not_found(PortName); + default: + throw unknown_error(); + } + } + + termios portSettings; + + memset( + &portSettings, + 0, + sizeof(termios)); + + tcflag_t baudrateFlag; + + switch (Baudrate) + { + case 9600: + baudrateFlag = B9600; + break; + case 19200: + baudrateFlag = B19200; + break; + case 38400: + baudrateFlag = B38400; + break; + case 57600: + baudrateFlag = B57600; + break; + case 115200: + baudrateFlag = B115200; + break; + + // QNX does not have higher baudrates defined. + #if !defined(__QNXNTO__) + + case 230400: + baudrateFlag = B230400; + break; + + // Not available on Mac OS X??? + #if !defined(__APPLE__) + + case 460800: + baudrateFlag = B460800; + break; + case 921600: + baudrateFlag = B921600; + break; + + #endif + + #endif + + default: + throw unknown_error(); + } + + // Set baudrate, 8n1, no modem control, and enable receiving characters. + #if __linux__ || __CYGWIN__ || __QNXNTO__ + portSettings.c_cflag = baudrateFlag; + #elif __APPLE__ + cfsetspeed(&portSettings, baudrateFlag); + #endif + portSettings.c_cflag |= CS8 | CLOCAL | CREAD; + + portSettings.c_iflag = IGNPAR; // Ignore bytes with parity errors. + portSettings.c_oflag = 0; // Enable raw data output. + portSettings.c_cc[VTIME] = 0; // Do not use inter-character timer. + portSettings.c_cc[VMIN] = 0; // Block on reads until 0 characters are received. + + // Clear the serial port buffers. + if (tcflush(portFd, TCIFLUSH) != 0) + throw unknown_error(); + + if (tcsetattr(portFd, TCSANOW, &portSettings) != 0) + throw unknown_error(); + + SerialPortHandle = portFd; + + #else + #error "Unknown System" + #endif + + if (checkAndToggleIsOpenFlag) + IsOpen = true; + + if (PurgeFirstDataBytesWhenSerialPortIsFirstOpened) + PurgeFirstDataBytesFromSerialPort(); + + StartSerialPortNotificationsThread(); + } +}; + +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable:4355) +#endif + +SerialPort::SerialPort( + const string &portName, + uint32_t baudrate) : + _pi(new Impl(this)) +{ + _pi->PortName = portName; + _pi->Baudrate = baudrate; +} + +#if defined (_MSC_VER) + #pragma warning(pop) +#endif + +SerialPort::~SerialPort() +{ + if (_pi->IsOpen) + { + try + { + close(); + } + catch (...) + { + // Something happened but don't want to throw out of the + // destructor. + } + } + + delete _pi; +} + +#if _WIN32 + +/// \brief Checks if the active serial port name provided is an FTDI USB serial +/// port. +/// +/// \return true if this is an FTDI USB serial port; otherwise false. +bool SerialPort_isFtdiUsbSerialPort(string portName) +{ + HDEVINFO deviceInfoSet = SetupDiGetClassDevs( + &GUID_DEVCLASS_PORTS, + NULL, + NULL, + DIGCF_PRESENT); + + if (deviceInfoSet == INVALID_HANDLE_VALUE) + throw unknown_error(); + + SP_DEVINFO_DATA deviceData; + ZeroMemory(&deviceData, sizeof(SP_DEVINFO_DATA)); + deviceData.cbSize = sizeof(SP_DEVINFO_DATA); + DWORD curDevId = 0; + + TCHAR portStrToFind[10]; + TCHAR cPortStr[10]; + + copy(portName.begin(), portName.end(), cPortStr); + cPortStr[portName.size()] = '\0'; + + #if VN_HAVE_SECURE_CRT + _stprintf_s(portStrToFind, sizeof(portStrToFind), TEXT("(%s)"), cPortStr); + #else + _stprintf(portStrToFind, TEXT("(%s)"), cPortStr); + #endif + + bool isFtdiDevice = false; + + while (SetupDiEnumDeviceInfo( + deviceInfoSet, + curDevId, + &deviceData)) + { + curDevId++; + TCHAR friendlyName[0x100]; + + if (!SetupDiGetDeviceRegistryProperty( + deviceInfoSet, + &deviceData, + SPDRP_FRIENDLYNAME, + NULL, + (PBYTE) friendlyName, + sizeof(friendlyName), + NULL)) + { + SetupDiDestroyDeviceInfoList(deviceInfoSet); + + throw unknown_error(); + } + + // See if this device is our COM port. + // TODO: There must be a better way to check the associated COM port number. + if (_tcsstr(friendlyName, portStrToFind) == NULL) + // Not the port we are looking for. + continue; + + // First see if this is an FTDI device. + TCHAR mfgName[0x100]; + if (!SetupDiGetDeviceRegistryProperty( + deviceInfoSet, + &deviceData, + SPDRP_MFG, + NULL, + (PBYTE) mfgName, + sizeof(mfgName), + NULL)) + { + SetupDiDestroyDeviceInfoList(deviceInfoSet); + + throw unknown_error(); + } + + // TODO: Possibly better way to check if this is an FTDI USB serial port. + isFtdiDevice = _tcscmp(mfgName, TEXT("FTDI")) == 0; + + break; + } + + SetupDiDestroyDeviceInfoList(deviceInfoSet); + + return isFtdiDevice; +} + +HKEY SerialPort_getRegistryKeyForActiveFtdiPort(string portName, bool isReadOnly) +{ + HDEVINFO deviceInfoSet = SetupDiGetClassDevs( + &GUID_DEVCLASS_PORTS, + NULL, + NULL, + DIGCF_PRESENT); + + if (deviceInfoSet == INVALID_HANDLE_VALUE) + throw unknown_error(); + + SP_DEVINFO_DATA deviceData; + ZeroMemory(&deviceData, sizeof(SP_DEVINFO_DATA)); + deviceData.cbSize = sizeof(SP_DEVINFO_DATA); + DWORD curDevId = 0; + + TCHAR portStrToFind[10]; + TCHAR cPortStr[10]; + + copy(portName.begin(), portName.end(), cPortStr); + cPortStr[portName.size()] = '\0'; + + #if VN_HAVE_SECURE_CRT + _stprintf_s(portStrToFind, sizeof(portStrToFind), TEXT("(%s)"), cPortStr); + #else + _stprintf(portStrToFind, TEXT("(%s)"), cPortStr); + #endif + + TCHAR deviceInstanceId[0x100]; + + while (SetupDiEnumDeviceInfo( + deviceInfoSet, + curDevId, + &deviceData)) + { + curDevId++; + TCHAR friendlyName[0x100]; + + if (!SetupDiGetDeviceRegistryProperty( + deviceInfoSet, + &deviceData, + SPDRP_FRIENDLYNAME, + NULL, + (PBYTE) friendlyName, + sizeof(friendlyName), + NULL)) + { + SetupDiDestroyDeviceInfoList(deviceInfoSet); + + throw unknown_error(); + } + + // See if this device is our COM port. + // TODO: There must be a better way to check the associated COM port number. + if (_tcsstr(friendlyName, portStrToFind) == NULL) + // Not the port we are looking for. + continue; + + // First see if this is an FTDI device. + TCHAR mfgName[0x100]; + if (!SetupDiGetDeviceRegistryProperty( + deviceInfoSet, + &deviceData, + SPDRP_MFG, + NULL, + (PBYTE) mfgName, + sizeof(mfgName), + NULL)) + { + SetupDiDestroyDeviceInfoList(deviceInfoSet); + + throw unknown_error(); + } + + if (_tcscmp(mfgName, TEXT("FTDI")) != 0) + { + // This COM port must not be and FTDI. + SetupDiDestroyDeviceInfoList(deviceInfoSet); + + throw invalid_operation(); + } + + // Found our port. Get the Device Instance ID/Name for later when we + // look in the registry. + if (!SetupDiGetDeviceInstanceId( + deviceInfoSet, + &deviceData, + deviceInstanceId, + sizeof(deviceInstanceId), + NULL)) + { + SetupDiDestroyDeviceInfoList(deviceInfoSet); + + throw unknown_error(); + } + + break; + } + + SetupDiDestroyDeviceInfoList(deviceInfoSet); + + // Now look in the registry for the FTDI entry. + HKEY ftdiKey; + TCHAR ftdiKeyPath[0x100]; + + #if VN_HAVE_SECURE_CRT + _stprintf_s(ftdiKeyPath, sizeof(ftdiKeyPath), TEXT("SYSTEM\\CurrentControlSet\\Enum\\%s\\Device Parameters"), deviceInstanceId); + #else + _stprintf(ftdiKeyPath, TEXT("SYSTEM\\CurrentControlSet\\Enum\\%s\\Device Parameters"), deviceInstanceId); + #endif + + REGSAM accessType = isReadOnly ? KEY_READ : KEY_READ | KEY_SET_VALUE; + + DWORD result = RegOpenKeyEx( + HKEY_LOCAL_MACHINE, + ftdiKeyPath, + 0, + accessType, + &ftdiKey); + + if (result == ERROR_ACCESS_DENIED) + throw permission_denied(); + + if (result != ERROR_SUCCESS) + throw unknown_error(); + + return ftdiKey; +} + +#endif + +bool SerialPort::determineIfPortIsOptimized(string portName) +{ + #if !_WIN32 + + // Don't know of any optimizations that need to be done for non-Windows systems. + return true; + + #else + + // We used to just search the the FTDI devices listed in the registry and + // locate the first entry that matched the requested portName. However, it + // is possible for multiple FTDI device listings to match the provided + // portName, probably from devices that are currently disconnected with the + // machine. The new technique first look through the PnP devices of the + // machine to first find which devices are active. + + if (!SerialPort_isFtdiUsbSerialPort(portName)) + // Only FTDI devices are known to require optimizing. + return true; + + HKEY ftdiKey = SerialPort_getRegistryKeyForActiveFtdiPort(portName, true); + + // Now see if the latency is set to 1. + DWORD latencyTimerValue; + DWORD latencyTimerValueSize = sizeof(latencyTimerValue); + + if (RegQueryValueEx( + ftdiKey, + TEXT("LatencyTimer"), + NULL, + NULL, + (LPBYTE) &latencyTimerValue, + &latencyTimerValueSize) != ERROR_SUCCESS) + { + throw unknown_error(); + } + + return latencyTimerValue == 1; + + #endif +} + +void SerialPort::optimizePort(string portName) +{ + #if !_WIN32 + + throw not_supported(); + + #else + + HKEY ftdiKey = SerialPort_getRegistryKeyForActiveFtdiPort(portName, false); + + DWORD latencyTimerValue = 1; + + if (RegSetValueEx( + ftdiKey, + TEXT("LatencyTimer"), + 0, + REG_DWORD, + (PBYTE) &latencyTimerValue, + sizeof(DWORD)) != ERROR_SUCCESS) + { + throw unknown_error(); + } + + #endif +} + +vector SerialPort::getPortNames() +{ + vector comPorts; + + #if _WIN32 + + HKEY serialCommKey; + LONG error; + + error = RegOpenKeyEx( + HKEY_LOCAL_MACHINE, + TEXT("HARDWARE\\DEVICEMAP\\SERIALCOMM"), + 0, + KEY_READ, + &serialCommKey); + + if (error != ERROR_SUCCESS) + throw unknown_error(); + + DWORD numOfSubkeys; + DWORD numOfValues; + + error = RegQueryInfoKey( + serialCommKey, + NULL, + NULL, + NULL, + &numOfSubkeys, + NULL, + NULL, + &numOfValues, + NULL, + NULL, + NULL, + NULL); + + if (error != ERROR_SUCCESS) + throw unknown_error(); + + for (size_t i = 0; i < numOfValues; i++) + { + TCHAR data[0x100]; + TCHAR value[0x100]; + DWORD capacity = 0x100; + DWORD dataSize = sizeof(data); + + error = RegEnumValue( + serialCommKey, + i, + value, + &capacity, + NULL, + NULL, + (LPBYTE) data, + &dataSize); + + if (error != ERROR_SUCCESS) + throw unknown_error(); + + #ifdef UNICODE + + char converted[0x100]; + int convertResult = WideCharToMultiByte( + CP_ACP, + 0, + data, + dataSize, + converted, + sizeof(converted), + NULL, + NULL); + + if (convertResult == 0) + throw unknown_error(); + + comPorts.push_back(string(converted)); + + #else + comPorts.push_back(string(data)); + #endif + } + + #elif __linux__ || __CYGWIN__ || __QNXNTO__ + + throw not_implemented(); + + #elif __APPLE__ + + DIR *dp = NULL; + struct dirent *dirp; + + if ((dp = opendir("/dev")) == NULL) + throw unknown_error(); + + while ((dirp = readdir(dp)) != NULL) + { + if (strstr(dirp->d_name, "tty.usbserial") != NULL) + comPorts.push_back(string(dirp->d_name)); + } + + closedir(dp); + + #else + #error "Unknown System" + #endif + + return comPorts; +} + +void SerialPort::open() +{ + _pi->open(); +} + +void SerialPort::close() +{ + #if PYTHON && PL156_FIX_ATTEMPT_1 + // Should this be in VnSensor disconnect or here? -Paul + python::ReleaseGIL t; + #endif + + _pi->close(); +} + +bool SerialPort::isOpen() +{ + return _pi->IsOpen; +} + +SerialPort::StopBits SerialPort::stopBits() +{ + return _pi->stopBits; +} + +void SerialPort::setStopBits(SerialPort::StopBits stopBits) +{ + _pi->ensureClosed(); + + _pi->stopBits = stopBits; +} + +void SerialPort::write(const char data[], size_t length) +{ + _pi->ensureOpened(); + + #if _WIN32 + + DWORD numOfBytesWritten; + BOOL result; + + OVERLAPPED overlapped; + memset(&overlapped, 0, sizeof(OVERLAPPED)); + + result = WriteFile( + _pi->SerialPortHandle, + data, + length, + NULL, + &overlapped); + + if (!result && GetLastError() != ERROR_IO_PENDING) + throw unknown_error(); + + result = GetOverlappedResult( + _pi->SerialPortHandle, + &overlapped, + reinterpret_cast(&numOfBytesWritten), + true); + + if (!result) + throw unknown_error(); + + result = FlushFileBuffers(_pi->SerialPortHandle); + + if (!result) + throw unknown_error(); + + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + ssize_t numOfBytesWritten = ::write( + _pi->SerialPortHandle, + data, + length); + + if (numOfBytesWritten == -1) + throw unknown_error(); + + #else + #error "Unknown System" + #endif +} + +void SerialPort::read(char dataBuffer[], size_t numOfBytesToRead, size_t &numOfBytesActuallyRead) +{ + _pi->ensureOpened(); + + #if _WIN32 + + OVERLAPPED overlapped; + memset(&overlapped, 0, sizeof(OVERLAPPED)); + + BOOL result = ReadFile( + _pi->SerialPortHandle, + dataBuffer, + numOfBytesToRead, + NULL, + &overlapped); + + if (!result && GetLastError() != ERROR_IO_PENDING) + throw unknown_error(); + + result = GetOverlappedResult( + _pi->SerialPortHandle, + &overlapped, + reinterpret_cast(&numOfBytesActuallyRead), + true); + + if (!result) + throw unknown_error(); + + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + int result = ::read( + _pi->SerialPortHandle, + dataBuffer, + numOfBytesToRead); + + if (result == -1) + throw unknown_error(); + + numOfBytesActuallyRead = static_cast(result); + + #else + #error "Unknown System" + #endif +} + +void SerialPort::registerDataReceivedHandler(void* userData, DataReceivedHandler handler) +{ + if (_pi->_dataReceivedHandler != NULL) + throw invalid_operation(); + + _pi->ObserversCriticalSection.enter(); + + _pi->_dataReceivedHandler = handler; + _pi->_dataReceivedUserData = userData; + + _pi->ObserversCriticalSection.leave(); +} + +void SerialPort::unregisterDataReceivedHandler() +{ + if (_pi->_dataReceivedHandler == NULL) + throw invalid_operation(); + + _pi->ObserversCriticalSection.enter(); + + _pi->_dataReceivedHandler = NULL; + _pi->_dataReceivedUserData = NULL; + + _pi->ObserversCriticalSection.leave(); +} + +uint32_t SerialPort::baudrate() +{ + return _pi->Baudrate; +} + +void SerialPort::changeBaudrate(uint32_t baudrate) +{ + _pi->ensureOpened(); + + _pi->close(false); + + _pi->Baudrate = baudrate; + + _pi->open(false); +} + +size_t SerialPort::NumberOfReceiveDataDroppedSections() +{ + #if _WIN32 + + return _pi->NumberOfReceiveDataDroppedSections; + + #elif __linux__ + + serial_icounter_struct serialStatus; + + ioctl( + _pi->SerialPortHandle, + TIOCGICOUNT, + &serialStatus); + + return serialStatus.overrun + serialStatus.buf_overrun; + + #elif __APPLE__ || __CYGWIN__ || __QNXNTO__ + + // Don't know how to implement this on Mac OS X. + throw not_implemented(); + + #else + #error "Unknown System" + #endif +} + +#if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1 + +void SerialPort::stopThread() +{ + _pi->externalStop(); +} + +bool SerialPort::threadStopped() +{ + return _pi->ThreadStopped; +} + +void SerialPort::resumeThread() +{ + _pi->externalStopFinished(); +} + +#endif + +} +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/xplat/thread.cpp b/ext/vnproglib-1.1.0.115/src/vn/xplat/thread.cpp new file mode 100755 index 0000000..82a8034 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/xplat/thread.cpp @@ -0,0 +1,184 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/xplat/thread.h" + +#if _WIN32 + #include +#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + #include + #include +#else + #error "Unknown System" +#endif + +#include "vn/exceptions.h" + +using namespace std; + +namespace vn { +namespace xplat { + +struct Thread::Impl +{ + #if _WIN32 + HANDLE ThreadHandle; + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + pthread_t ThreadHandle; + void* Data; + #else + #error "Unknown System" + #endif + + Thread::ThreadStartRoutine StartRoutine; + + Impl() : + #if _WIN32 + ThreadHandle(NULL), + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + ThreadHandle(0), + Data(NULL), + #else + #error "Unknown System" + #endif + StartRoutine(NULL) + { } + + #if __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + static void* StartRoutineWrapper(void* data) + { + Impl* impl = (Impl*) data; + + impl->StartRoutine(impl->Data); + + return NULL; + } + + #endif +}; + +Thread::Thread( + ThreadStartRoutine startRoutine) : + _pimpl(new Thread::Impl()) +{ + _pimpl->StartRoutine = startRoutine; +} + +Thread::~Thread() +{ + delete _pimpl; +} + +Thread* Thread::startNew(ThreadStartRoutine startRoutine, void* routineData) +{ + Thread* newThread = new Thread(startRoutine); + + newThread->start(routineData); + + return newThread; +} + +void Thread::start(void* routineData) +{ + if (_pimpl->StartRoutine != NULL) + { + #if _WIN32 + + _pimpl->ThreadHandle = CreateThread( + NULL, + 0, + (LPTHREAD_START_ROUTINE) _pimpl->StartRoutine, + routineData, + 0, + NULL); + + if (_pimpl->ThreadHandle == NULL) + throw unknown_error(); + + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + _pimpl->Data = routineData; + + int errorCode = pthread_create( + &_pimpl->ThreadHandle, + NULL, + Impl::StartRoutineWrapper, + _pimpl); + + if (errorCode != 0) + throw unknown_error(); + + #else + #error "Unknown System" + #endif + } + else + { + throw not_implemented(); + } +} + +void Thread::join() +{ + #if _WIN32 + + WaitForSingleObject( + _pimpl->ThreadHandle, + INFINITE); + + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + if (pthread_join( + _pimpl->ThreadHandle, + NULL)) + throw unknown_error(); + + #else + #error "Unknown System" + #endif +} + +void Thread::sleepSec(uint32_t numOfSecsToSleep) +{ + #if _WIN32 + Sleep(numOfSecsToSleep * 1000); + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + sleep(numOfSecsToSleep); + #else + #error "Unknown System" + #endif +} + +void Thread::sleepMs(uint32_t numOfMsToSleep) +{ + #if _WIN32 + Sleep(numOfMsToSleep); + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + usleep(numOfMsToSleep * 1000); + #else + #error "Unknown System" + #endif +} + +void Thread::sleepUs(uint32_t numOfUsToSleep) +{ + #if _WIN32 + throw not_implemented(); + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + usleep(numOfUsToSleep); + #else + #error "Unknown System" + #endif +} + +void Thread::sleepNs(uint32_t numOfNsToSleep) +{ + #if _WIN32 + throw not_implemented(); + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + throw not_implemented(); + #endif +} + +} +} diff --git a/ext/vnproglib-1.1.0.115/src/vn/xplat/time.cpp b/ext/vnproglib-1.1.0.115/src/vn/xplat/time.cpp new file mode 100755 index 0000000..b4ead77 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/src/vn/xplat/time.cpp @@ -0,0 +1,183 @@ +// VectorNav Programming Library v1.1.0.115 +// Copyright (c) 2016 VectorNav Technologies, LLC +#include "vn/xplat/time.h" + +#if _WIN32 + #include +#elif __linux__ || __CYGWIN__ || __QNXNTO__ + #include + #include +#elif __APPLE__ + #include + #include +#else + #error "Unknown System" +#endif + +#include "vn/exceptions.h" + +using namespace std; + +namespace vn { +namespace xplat { + +TimeStamp::TimeStamp() : _sec(0), _usec(0) { } + +TimeStamp::TimeStamp(int64_t sec, uint64_t usec) : _sec(sec), _usec(usec) { } + +TimeStamp TimeStamp::get() +{ + #if _WIN32 + + // HACK: Just returning an empty TimeStamp for now. + return TimeStamp(); + + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + + struct timeval tv; + + gettimeofday(&tv, NULL); + + return TimeStamp(tv.tv_sec, tv.tv_usec); + + #else + #error "Unknown System" + #endif +} + +struct Stopwatch::Impl +{ + #if _WIN32 + double _pcFrequency; + __int64 _counterStart; + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + double _clockStart; + #else + #error "Unknown System" + #endif + + Impl() : + #if _WIN32 + _pcFrequency(0), + _counterStart(-1) + #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ + _clockStart(-1) + #else + #error "Unknown System" + #endif + { + // Start the stopwatch. + reset(); + } + + void reset() + { + #if _WIN32 + + LARGE_INTEGER li; + if(!QueryPerformanceFrequency(&li)) + // The hardware must not support a high-resolution performance counter. + throw not_supported(); + + _pcFrequency = static_cast(li.QuadPart) / 1000.0; + + QueryPerformanceCounter(&li); + + _counterStart = li.QuadPart; + + #elif __linux__ || __CYGWIN__ || __QNXNTO__ + + struct timespec time; + int error; + + error = clock_gettime(CLOCK_MONOTONIC, &time); + + if (error) + throw unknown_error(); + + _clockStart = (time.tv_sec * 1000.0) + (time.tv_nsec / 1000000.0); + + #elif __APPLE__ + + clock_serv_t cclock; + mach_timespec_t mts; + + host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock); + clock_get_time(cclock, &mts); + mach_port_deallocate(mach_task_self(), cclock); + + _clockStart = (mts.tv_sec * 1000.0) + (mts.tv_nsec / 1000000.0); + + #else + #error "Unknown System" + #endif + } + + float elapsedMs() + { + #if _WIN32 + + LARGE_INTEGER li; + + if (_counterStart == -1) + throw unknown_error(); + + QueryPerformanceCounter(&li); + + return static_cast((static_cast(li.QuadPart) - _counterStart) / _pcFrequency); + + #elif __linux__ || __CYGWIN__ || __QNXNTO__ + + struct timespec time; + int error; + + if (_clockStart < 0) + // Clock not started. + throw invalid_operation(); + + error = clock_gettime(CLOCK_MONOTONIC, &time); + + if (error) + throw unknown_error(); + + return (time.tv_sec * 1000.0) + (time.tv_nsec / 1000000.0) - _clockStart; + + #elif __APPLE__ + + clock_serv_t cclock; + mach_timespec_t mts; + + host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock); + clock_get_time(cclock, &mts); + mach_port_deallocate(mach_task_self(), cclock); + + return (mts.tv_sec * 1000.0) + (mts.tv_nsec / 1000000.0) - _clockStart; + + #else + #error "Unknown System" + #endif + } +}; + +Stopwatch::Stopwatch() : + _pi(new Impl()) +{ +} + +Stopwatch::~Stopwatch() +{ + delete _pi; +} + +void Stopwatch::reset() +{ + _pi->reset(); +} + +float Stopwatch::elapsedMs() +{ + return _pi->elapsedMs(); +} + +} +} diff --git a/src/imu_vn_100.cpp b/src/imu_vn_100.cpp index 3c16bf4..79084e1 100644 --- a/src/imu_vn_100.cpp +++ b/src/imu_vn_100.cpp @@ -214,7 +214,7 @@ void ImuVn100::Stream(bool async) { if (binary_output_) { // Set the binary output data type and data rate VnEnsure(vn100_setBinaryOutput1Configuration( - &imu_, BINARY_ASYNC_MODE_SERIAL_2, kBaseImuRate / imu_rate_, + &imu_, BINARY_ASYNC_MODE_SERIAL_1, kBaseImuRate / imu_rate_, BG1_QTN | BG1_IMU | BG1_MAG_PRES | BG1_SYNC_IN_CNT, // BG1_IMU, BG3_NONE, BG5_NONE, true)); From 82c199763470ec27785a5172ce08f1be6d223309 Mon Sep 17 00:00:00 2001 From: Andre Nguyen Date: Thu, 25 Feb 2016 18:56:12 -0500 Subject: [PATCH 02/10] add skeleton --- CMakeLists.txt | 17 +- .../include/vn/math/kinematics.h | 2 +- include/imu_vn_100/imu_vn_100.h | 14 +- include/imu_vn_100/imu_vn_100_test.h | 138 +++++++ src/imu_vn_100.cpp | 290 ++------------- src/imu_vn_100_test.cpp | 352 ++++++++++++++++++ 6 files changed, 541 insertions(+), 272 deletions(-) create mode 100644 include/imu_vn_100/imu_vn_100_test.h create mode 100644 src/imu_vn_100_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 12e77d7..73cd37f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 2.8.3) project(imu_vn_100) +# build the vnproflib first add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/ext/vnproglib-1.1.0.115) + add_definitions("-std=c++11") find_package(catkin REQUIRED COMPONENTS @@ -20,19 +22,24 @@ catkin_package( include_directories( include - vncpplib/include +# vncpplib/include ext/vnproglib-1.1.0.115/include ${catkin_INCLUDE_DIRS} ) +#add_library(imu_vn_100 +# vncpplib/src/arch/linux/vncp_services.c +# vncpplib/src/vndevice.c +# vncpplib/src/vn100.c +# src/imu_vn_100.cpp +#) + add_library(imu_vn_100 - vncpplib/src/arch/linux/vncp_services.c - vncpplib/src/vndevice.c - vncpplib/src/vn100.c - src/imu_vn_100.cpp + src/imu_vn_100.cpp ) target_link_libraries(imu_vn_100 + vnproglib ${catkin_LIBRARIES} ) diff --git a/ext/vnproglib-1.1.0.115/include/vn/math/kinematics.h b/ext/vnproglib-1.1.0.115/include/vn/math/kinematics.h index 9745f94..36cc42c 100755 --- a/ext/vnproglib-1.1.0.115/include/vn/math/kinematics.h +++ b/ext/vnproglib-1.1.0.115/include/vn/math/kinematics.h @@ -84,7 +84,7 @@ struct ypr /// \return The \ref yaw value in degrees. T yawDegs() const { - return math::rad2deg(Yyaw); + return math::rad2deg(yaw); } /// \brief Returns the \ref pitch value in degrees. diff --git a/include/imu_vn_100/imu_vn_100.h b/include/imu_vn_100/imu_vn_100.h index ec49537..1a8b282 100644 --- a/include/imu_vn_100/imu_vn_100.h +++ b/include/imu_vn_100/imu_vn_100.h @@ -1,5 +1,7 @@ /* - * Copyright [2015] [Ke Sun] + * Copyright [2016] + * Authors: [Ke Sun] + * Andre Phu-Van Nguyen * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -25,8 +27,6 @@ #include #include -#include - namespace imu_vn_100 { namespace du = diagnostic_updater; @@ -75,7 +75,7 @@ class ImuVn100 { void Stream(bool async = true); - void PublishData(const VnDeviceCompositeData& data); +// void PublishData(const VnDeviceCompositeData& data); void RequestOnce(); @@ -105,7 +105,7 @@ class ImuVn100 { private: ros::NodeHandle pnh_; - Vn100 imu_; + //Vn100 imu_; // Settings std::string port_; @@ -130,8 +130,8 @@ class ImuVn100 { }; // Just don't like type that is ALL CAP -using VnErrorCode = VN_ERROR_CODE; -void VnEnsure(const VnErrorCode& error_code); +//using VnErrorCode = VN_ERROR_CODE; +//void VnEnsure(const VnErrorCode& error_code); } // namespace imu_vn_100 diff --git a/include/imu_vn_100/imu_vn_100_test.h b/include/imu_vn_100/imu_vn_100_test.h new file mode 100644 index 0000000..ec49537 --- /dev/null +++ b/include/imu_vn_100/imu_vn_100_test.h @@ -0,0 +1,138 @@ +/* + * Copyright [2015] [Ke Sun] + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef IMU_VN_100_ROS_H_ +#define IMU_VN_100_ROS_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace imu_vn_100 { + +namespace du = diagnostic_updater; +using TopicDiagnosticPtr = boost::shared_ptr; + +// NOTE: there is a DiagnosedPublisher inside diagnostic_updater +// but it does not have a default constructor thus we use this simple one +// instead, which has the same functionality +struct DiagnosedPublisher { + ros::Publisher pub; + TopicDiagnosticPtr diag; + + template + void Create(ros::NodeHandle& pnh, const std::string& topic, + du::Updater& updater, double& rate) { + pub = pnh.advertise(topic, 1); + du::FrequencyStatusParam freq_param(&rate, &rate, 0.01, 10); + du::TimeStampStatusParam time_param(0, 0.5 / rate); + diag = boost::make_shared(topic, updater, freq_param, + time_param); + } + + template + void Publish(const MessageT& message) { + diag->tick(message.header.stamp); + pub.publish(message); + } +}; + +/** + * @brief ImuVn100 The class is a ros wrapper for the Imu class + * @author Ke Sun + */ +class ImuVn100 { + public: + static constexpr int kBaseImuRate = 800; + static constexpr int kDefaultImuRate = 100; + static constexpr int kDefaultSyncOutRate = 20; + + explicit ImuVn100(const ros::NodeHandle& pnh); + ImuVn100(const ImuVn100&) = delete; + ImuVn100& operator=(const ImuVn100&) = delete; + ~ImuVn100(); + + void Initialize(); + + void Stream(bool async = true); + + void PublishData(const VnDeviceCompositeData& data); + + void RequestOnce(); + + void Idle(bool need_reply = true); + + void Resume(bool need_reply = true); + + void Disconnect(); + + void Configure(); + + struct SyncInfo { + unsigned count = 0; + ros::Time time; + + int rate = -1; + double rate_double = -1; + int pulse_width_us = 1000; + int skip_count = 0; + + void Update(const unsigned sync_count, const ros::Time& sync_time); + void FixSyncRate(); + bool SyncEnabled() const; + }; + + const SyncInfo sync_info() const { return sync_info_; } + + private: + ros::NodeHandle pnh_; + Vn100 imu_; + + // Settings + std::string port_; + int baudrate_ = 921600; + int imu_rate_ = kDefaultImuRate; + double imu_rate_double_ = kDefaultImuRate; + std::string frame_id_; + + bool enable_mag_ = true; + bool enable_pres_ = true; + bool enable_temp_ = true; + bool binary_output_ = true; + + SyncInfo sync_info_; + + du::Updater updater_; + DiagnosedPublisher pd_imu_, pd_mag_, pd_pres_, pd_temp_; + + void FixImuRate(); + void LoadParameters(); + void CreateDiagnosedPublishers(); +}; + +// Just don't like type that is ALL CAP +using VnErrorCode = VN_ERROR_CODE; +void VnEnsure(const VnErrorCode& error_code); + +} // namespace imu_vn_100 + +#endif // IMU_VN_100_ROS_H_ diff --git a/src/imu_vn_100.cpp b/src/imu_vn_100.cpp index 79084e1..3b16ff0 100644 --- a/src/imu_vn_100.cpp +++ b/src/imu_vn_100.cpp @@ -1,5 +1,7 @@ /* - * Copyright [2015] [Ke Sun] + * Copyright 2016 + * Authors: [Ke Sun] + * Andre Phu-Van Nguyen * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -15,27 +17,27 @@ */ #include +#include "vn/math/math.h" +#include "vn/math/conversions.h" +#include "vn/math/kinematics.h" namespace imu_vn_100 { -// LESS HACK IS STILL HACK -ImuVn100* imu_vn_100_ptr; - -using sensor_msgs::Imu; -using sensor_msgs::MagneticField; -using sensor_msgs::FluidPressure; -using sensor_msgs::Temperature; - +/** + * @brief RosVector3FromVnVector3 + * @param ros_vec3 + * @param vn_vec3 + */ void RosVector3FromVnVector3(geometry_msgs::Vector3& ros_vec3, - const VnVector3& vn_vec3); -void RosQuaternionFromVnQuaternion(geometry_msgs::Quaternion& ros_quat, - const VnQuaternion& vn_quat); -void FillImuMessage(sensor_msgs::Imu& imu_msg, - const VnDeviceCompositeData& data, bool binary_output); + const vn::math::vec3f& vn_vec3); -void AsyncListener(void* sender, VnDeviceCompositeData* data) { - imu_vn_100_ptr->PublishData(*data); -} +/** + * @brief RosQuaternionFromVnQuaternion + * @param ros_quat + * @param vn_quat + */ +void RosQuaternionFromVnQuaternion(geometry_msgs::Quaternion& ros_quat, + const vn::math::kinematics::quat& vn_quat); constexpr int ImuVn100::kBaseImuRate; constexpr int ImuVn100::kDefaultImuRate; @@ -75,278 +77,48 @@ void ImuVn100::SyncInfo::FixSyncRate() { ROS_INFO("Sync out rate: %d", rate); } -ImuVn100::ImuVn100(const ros::NodeHandle& pnh) - : pnh_(pnh), - port_(std::string("/dev/ttyUSB0")), - baudrate_(921600), - frame_id_(std::string("imu")) { - Initialize(); - imu_vn_100_ptr = this; +ImuVn100::ImuVn100(const ros::NodeHandle& pnh){ + } -ImuVn100::~ImuVn100() { Disconnect(); } +ImuVn100::~ImuVn100() { + Disconnect(); +} void ImuVn100::FixImuRate() { - if (imu_rate_ <= 0) { - ROS_WARN("Imu rate %d is < 0. Set to %d", imu_rate_, kDefaultImuRate); - imu_rate_ = kDefaultImuRate; - } - if (kBaseImuRate % imu_rate_ != 0) { - int imu_rate_old = imu_rate_; - imu_rate_ = kBaseImuRate / (kBaseImuRate / imu_rate_old); - ROS_WARN("Imu rate %d cannot evenly decimate base rate %d, reset to %d", - imu_rate_old, kBaseImuRate, imu_rate_); - } } void ImuVn100::LoadParameters() { - pnh_.param("port", port_, std::string("/dev/ttyUSB0")); - pnh_.param("frame_id", frame_id_, pnh_.getNamespace()); - pnh_.param("baudrate", baudrate_, 115200); - pnh_.param("imu_rate", imu_rate_, kDefaultImuRate); - - pnh_.param("enable_mag", enable_mag_, true); - pnh_.param("enable_pres", enable_pres_, true); - pnh_.param("enable_temp", enable_temp_, true); - pnh_.param("sync_rate", sync_info_.rate, kDefaultSyncOutRate); - pnh_.param("sync_pulse_width_us", sync_info_.pulse_width_us, 1000); - - pnh_.param("binary_output", binary_output_, true); - - FixImuRate(); - sync_info_.FixSyncRate(); } void ImuVn100::CreateDiagnosedPublishers() { - imu_rate_double_ = imu_rate_; - pd_imu_.Create(pnh_, "imu", updater_, imu_rate_double_); - if (enable_mag_) { - pd_mag_.Create(pnh_, "magnetic_field", updater_, - imu_rate_double_); - } - if (enable_pres_) { - pd_pres_.Create(pnh_, "fluid_pressure", updater_, - imu_rate_double_); - } - if (enable_temp_) { - pd_temp_.Create(pnh_, "temperature", updater_, - imu_rate_double_); - } + } void ImuVn100::Initialize() { - LoadParameters(); - - ROS_DEBUG("Connecting to device"); - VnEnsure(vn100_connect(&imu_, port_.c_str(), 115200)); - ros::Duration(0.5).sleep(); - ROS_INFO("Connected to device at %s", port_.c_str()); - - unsigned int old_baudrate; - VnEnsure(vn100_getSerialBaudRate(&imu_, &old_baudrate)); - ROS_INFO("Default serial baudrate: %u", old_baudrate); - - ROS_INFO("Set serial baudrate to %d", baudrate_); - VnEnsure(vn100_setSerialBaudRate(&imu_, baudrate_, true)); - - ROS_DEBUG("Disconnecting the device"); - vn100_disconnect(&imu_); - ros::Duration(0.5).sleep(); - - ROS_DEBUG("Reconnecting to device"); - VnEnsure(vn100_connect(&imu_, port_.c_str(), baudrate_)); - ros::Duration(0.5).sleep(); - ROS_INFO("Connected to device at %s", port_.c_str()); - - VnEnsure(vn100_getSerialBaudRate(&imu_, &old_baudrate)); - ROS_INFO("New serial baudrate: %u", old_baudrate); - - // Idle the device for intialization - VnEnsure(vn100_pauseAsyncOutputs(&imu_, true)); - - ROS_INFO("Fetching device info."); - char model_number_buffer[30] = {0}; - int hardware_revision = 0; - char serial_number_buffer[30] = {0}; - char firmware_version_buffer[30] = {0}; - - VnEnsure(vn100_getModelNumber(&imu_, model_number_buffer, 30)); - ROS_INFO("Model number: %s", model_number_buffer); - VnEnsure(vn100_getHardwareRevision(&imu_, &hardware_revision)); - ROS_INFO("Hardware revision: %d", hardware_revision); - VnEnsure(vn100_getSerialNumber(&imu_, serial_number_buffer, 30)); - ROS_INFO("Serial number: %s", serial_number_buffer); - VnEnsure(vn100_getFirmwareVersion(&imu_, firmware_version_buffer, 30)); - ROS_INFO("Firmware version: %s", firmware_version_buffer); - - if (sync_info_.SyncEnabled()) { - ROS_INFO("Set Synchronization Control Register (id:32)."); - VnEnsure(vn100_setSynchronizationControl( - &imu_, SYNCINMODE_COUNT, SYNCINEDGE_RISING, 0, SYNCOUTMODE_IMU_START, - SYNCOUTPOLARITY_POSITIVE, sync_info_.skip_count, - sync_info_.pulse_width_us * 1000, true)); - - if (!binary_output_) { - ROS_INFO("Set Communication Protocal Control Register (id:30)."); - VnEnsure(vn100_setCommunicationProtocolControl( - &imu_, SERIALCOUNT_SYNCOUT_COUNT, SERIALSTATUS_OFF, SPICOUNT_NONE, - SPISTATUS_OFF, SERIALCHECKSUM_8BIT, SPICHECKSUM_8BIT, ERRORMODE_SEND, - true)); - } - } - - CreateDiagnosedPublishers(); - auto hardware_id = std::string("vn100-") + std::string(model_number_buffer) + - std::string(serial_number_buffer); - updater_.setHardwareID(hardware_id); } -void ImuVn100::Stream(bool async) { - // Pause the device first - VnEnsure(vn100_pauseAsyncOutputs(&imu_, true)); - - if (async) { - VnEnsure(vn100_setAsynchronousDataOutputType(&imu_, VNASYNC_OFF, true)); - - if (binary_output_) { - // Set the binary output data type and data rate - VnEnsure(vn100_setBinaryOutput1Configuration( - &imu_, BINARY_ASYNC_MODE_SERIAL_1, kBaseImuRate / imu_rate_, - BG1_QTN | BG1_IMU | BG1_MAG_PRES | BG1_SYNC_IN_CNT, - // BG1_IMU, - BG3_NONE, BG5_NONE, true)); - } else { - // Set the ASCII output data type and data rate - // ROS_INFO("Configure the output data type and frequency (id: 6 & 7)"); - VnEnsure(vn100_setAsynchronousDataOutputType(&imu_, VNASYNC_VNIMU, true)); - } - - // Add a callback function for new data event - VnEnsure(vn100_registerAsyncDataReceivedListener(&imu_, &AsyncListener)); - - ROS_INFO("Setting IMU rate to %d", imu_rate_); - VnEnsure(vn100_setAsynchronousDataOutputFrequency(&imu_, imu_rate_, true)); - } else { - // Mute the stream - ROS_DEBUG("Mute the device"); - VnEnsure(vn100_setAsynchronousDataOutputType(&imu_, VNASYNC_OFF, true)); - // Remove the callback function for new data event - VnEnsure(vn100_unregisterAsyncDataReceivedListener(&imu_, &AsyncListener)); - } +void ImuVn100::Stream(bool async){ - // Resume the device - VnEnsure(vn100_resumeAsyncOutputs(&imu_, true)); } void ImuVn100::Resume(bool need_reply) { - vn100_resumeAsyncOutputs(&imu_, need_reply); -} - -void ImuVn100::Idle(bool need_reply) { - vn100_pauseAsyncOutputs(&imu_, need_reply); -} -void ImuVn100::Disconnect() { - // TODO: why reset the device? - vn100_reset(&imu_); - vn100_disconnect(&imu_); } -void ImuVn100::PublishData(const VnDeviceCompositeData& data) { - sensor_msgs::Imu imu_msg; - imu_msg.header.stamp = ros::Time::now(); - imu_msg.header.frame_id = frame_id_; - - FillImuMessage(imu_msg, data, binary_output_); - pd_imu_.Publish(imu_msg); - - if (enable_mag_) { - sensor_msgs::MagneticField mag_msg; - mag_msg.header = imu_msg.header; - RosVector3FromVnVector3(mag_msg.magnetic_field, data.magnetic); - pd_mag_.Publish(mag_msg); - } - - if (enable_pres_) { - sensor_msgs::FluidPressure pres_msg; - pres_msg.header = imu_msg.header; - pres_msg.fluid_pressure = data.pressure; - pd_pres_.Publish(pres_msg); - } - - if (enable_temp_) { - sensor_msgs::Temperature temp_msg; - temp_msg.header = imu_msg.header; - temp_msg.temperature = data.temperature; - pd_temp_.Publish(temp_msg); - } - - sync_info_.Update(data.syncInCnt, imu_msg.header.stamp); +void ImuVn100::Idle(bool need_reply) { - updater_.update(); } -void VnEnsure(const VnErrorCode& error_code) { - if (error_code == VNERR_NO_ERROR) return; - - switch (error_code) { - case VNERR_UNKNOWN_ERROR: - throw std::runtime_error("VN: Unknown error"); - case VNERR_NOT_IMPLEMENTED: - throw std::runtime_error("VN: Not implemented"); - case VNERR_TIMEOUT: - ROS_WARN("Opertation time out"); - break; - case VNERR_SENSOR_INVALID_PARAMETER: - ROS_WARN("VN: Sensor invalid paramter"); - break; - case VNERR_INVALID_VALUE: - ROS_WARN("VN: Invalid value"); - break; - case VNERR_FILE_NOT_FOUND: - ROS_WARN("VN: File not found"); - break; - case VNERR_NOT_CONNECTED: - throw std::runtime_error("VN: not connected"); - case VNERR_PERMISSION_DENIED: - throw std::runtime_error("VN: Permission denied"); - default: - ROS_WARN("Unhandled error type"); - } -} +void ImuVn100::Disconnect() { -void RosVector3FromVnVector3(geometry_msgs::Vector3& ros_vec3, - const VnVector3& vn_vec3) { - ros_vec3.x = vn_vec3.c0; - ros_vec3.y = vn_vec3.c1; - ros_vec3.z = vn_vec3.c2; } +/* +void ImuVn100::PublishData(const VnDeviceCompositeData& data) { -void RosQuaternionFromVnQuaternion(geometry_msgs::Quaternion& ros_quat, - const VnQuaternion& vn_quat) { - ros_quat.x = vn_quat.x; - ros_quat.y = vn_quat.y; - ros_quat.z = vn_quat.z; - ros_quat.w = vn_quat.w; -} +}*/ -void FillImuMessage(sensor_msgs::Imu& imu_msg, - const VnDeviceCompositeData& data, bool binary_output) { - if (binary_output) { - RosQuaternionFromVnQuaternion(imu_msg.orientation, data.quaternion); - // NOTE: The IMU angular velocity and linear acceleration outputs are - // swapped. And also why are they different? - RosVector3FromVnVector3(imu_msg.angular_velocity, - data.accelerationUncompensated); - RosVector3FromVnVector3(imu_msg.linear_acceleration, - data.angularRateUncompensated); - } else { - RosVector3FromVnVector3(imu_msg.linear_acceleration, data.acceleration); - RosVector3FromVnVector3(imu_msg.angular_velocity, data.angularRate); - } } - -} // namespace imu_vn_100 diff --git a/src/imu_vn_100_test.cpp b/src/imu_vn_100_test.cpp new file mode 100644 index 0000000..79084e1 --- /dev/null +++ b/src/imu_vn_100_test.cpp @@ -0,0 +1,352 @@ +/* + * Copyright [2015] [Ke Sun] + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +namespace imu_vn_100 { + +// LESS HACK IS STILL HACK +ImuVn100* imu_vn_100_ptr; + +using sensor_msgs::Imu; +using sensor_msgs::MagneticField; +using sensor_msgs::FluidPressure; +using sensor_msgs::Temperature; + +void RosVector3FromVnVector3(geometry_msgs::Vector3& ros_vec3, + const VnVector3& vn_vec3); +void RosQuaternionFromVnQuaternion(geometry_msgs::Quaternion& ros_quat, + const VnQuaternion& vn_quat); +void FillImuMessage(sensor_msgs::Imu& imu_msg, + const VnDeviceCompositeData& data, bool binary_output); + +void AsyncListener(void* sender, VnDeviceCompositeData* data) { + imu_vn_100_ptr->PublishData(*data); +} + +constexpr int ImuVn100::kBaseImuRate; +constexpr int ImuVn100::kDefaultImuRate; +constexpr int ImuVn100::kDefaultSyncOutRate; + +void ImuVn100::SyncInfo::Update(const unsigned sync_count, + const ros::Time& sync_time) { + if (rate <= 0) return; + + if (count != sync_count) { + count = sync_count; + time = sync_time; + } +} + +bool ImuVn100::SyncInfo::SyncEnabled() const { return rate > 0; } + +void ImuVn100::SyncInfo::FixSyncRate() { + // Check the sync out rate + if (SyncEnabled()) { + if (ImuVn100::kBaseImuRate % rate != 0) { + rate = ImuVn100::kBaseImuRate / (ImuVn100::kBaseImuRate / rate); + ROS_INFO("Set SYNC_OUT_RATE to %d", rate); + } + skip_count = + (std::floor(ImuVn100::kBaseImuRate / static_cast(rate) + + 0.5f)) - + 1; + + if (pulse_width_us > 10000) { + ROS_INFO("Sync out pulse with is over 10ms. Reset to 1ms"); + pulse_width_us = 1000; + } + rate_double = rate; + } + + ROS_INFO("Sync out rate: %d", rate); +} + +ImuVn100::ImuVn100(const ros::NodeHandle& pnh) + : pnh_(pnh), + port_(std::string("/dev/ttyUSB0")), + baudrate_(921600), + frame_id_(std::string("imu")) { + Initialize(); + imu_vn_100_ptr = this; +} + +ImuVn100::~ImuVn100() { Disconnect(); } + +void ImuVn100::FixImuRate() { + if (imu_rate_ <= 0) { + ROS_WARN("Imu rate %d is < 0. Set to %d", imu_rate_, kDefaultImuRate); + imu_rate_ = kDefaultImuRate; + } + + if (kBaseImuRate % imu_rate_ != 0) { + int imu_rate_old = imu_rate_; + imu_rate_ = kBaseImuRate / (kBaseImuRate / imu_rate_old); + ROS_WARN("Imu rate %d cannot evenly decimate base rate %d, reset to %d", + imu_rate_old, kBaseImuRate, imu_rate_); + } +} + +void ImuVn100::LoadParameters() { + pnh_.param("port", port_, std::string("/dev/ttyUSB0")); + pnh_.param("frame_id", frame_id_, pnh_.getNamespace()); + pnh_.param("baudrate", baudrate_, 115200); + pnh_.param("imu_rate", imu_rate_, kDefaultImuRate); + + pnh_.param("enable_mag", enable_mag_, true); + pnh_.param("enable_pres", enable_pres_, true); + pnh_.param("enable_temp", enable_temp_, true); + + pnh_.param("sync_rate", sync_info_.rate, kDefaultSyncOutRate); + pnh_.param("sync_pulse_width_us", sync_info_.pulse_width_us, 1000); + + pnh_.param("binary_output", binary_output_, true); + + FixImuRate(); + sync_info_.FixSyncRate(); +} + +void ImuVn100::CreateDiagnosedPublishers() { + imu_rate_double_ = imu_rate_; + pd_imu_.Create(pnh_, "imu", updater_, imu_rate_double_); + if (enable_mag_) { + pd_mag_.Create(pnh_, "magnetic_field", updater_, + imu_rate_double_); + } + if (enable_pres_) { + pd_pres_.Create(pnh_, "fluid_pressure", updater_, + imu_rate_double_); + } + if (enable_temp_) { + pd_temp_.Create(pnh_, "temperature", updater_, + imu_rate_double_); + } +} + +void ImuVn100::Initialize() { + LoadParameters(); + + ROS_DEBUG("Connecting to device"); + VnEnsure(vn100_connect(&imu_, port_.c_str(), 115200)); + ros::Duration(0.5).sleep(); + ROS_INFO("Connected to device at %s", port_.c_str()); + + unsigned int old_baudrate; + VnEnsure(vn100_getSerialBaudRate(&imu_, &old_baudrate)); + ROS_INFO("Default serial baudrate: %u", old_baudrate); + + ROS_INFO("Set serial baudrate to %d", baudrate_); + VnEnsure(vn100_setSerialBaudRate(&imu_, baudrate_, true)); + + ROS_DEBUG("Disconnecting the device"); + vn100_disconnect(&imu_); + ros::Duration(0.5).sleep(); + + ROS_DEBUG("Reconnecting to device"); + VnEnsure(vn100_connect(&imu_, port_.c_str(), baudrate_)); + ros::Duration(0.5).sleep(); + ROS_INFO("Connected to device at %s", port_.c_str()); + + VnEnsure(vn100_getSerialBaudRate(&imu_, &old_baudrate)); + ROS_INFO("New serial baudrate: %u", old_baudrate); + + // Idle the device for intialization + VnEnsure(vn100_pauseAsyncOutputs(&imu_, true)); + + ROS_INFO("Fetching device info."); + char model_number_buffer[30] = {0}; + int hardware_revision = 0; + char serial_number_buffer[30] = {0}; + char firmware_version_buffer[30] = {0}; + + VnEnsure(vn100_getModelNumber(&imu_, model_number_buffer, 30)); + ROS_INFO("Model number: %s", model_number_buffer); + VnEnsure(vn100_getHardwareRevision(&imu_, &hardware_revision)); + ROS_INFO("Hardware revision: %d", hardware_revision); + VnEnsure(vn100_getSerialNumber(&imu_, serial_number_buffer, 30)); + ROS_INFO("Serial number: %s", serial_number_buffer); + VnEnsure(vn100_getFirmwareVersion(&imu_, firmware_version_buffer, 30)); + ROS_INFO("Firmware version: %s", firmware_version_buffer); + + if (sync_info_.SyncEnabled()) { + ROS_INFO("Set Synchronization Control Register (id:32)."); + VnEnsure(vn100_setSynchronizationControl( + &imu_, SYNCINMODE_COUNT, SYNCINEDGE_RISING, 0, SYNCOUTMODE_IMU_START, + SYNCOUTPOLARITY_POSITIVE, sync_info_.skip_count, + sync_info_.pulse_width_us * 1000, true)); + + if (!binary_output_) { + ROS_INFO("Set Communication Protocal Control Register (id:30)."); + VnEnsure(vn100_setCommunicationProtocolControl( + &imu_, SERIALCOUNT_SYNCOUT_COUNT, SERIALSTATUS_OFF, SPICOUNT_NONE, + SPISTATUS_OFF, SERIALCHECKSUM_8BIT, SPICHECKSUM_8BIT, ERRORMODE_SEND, + true)); + } + } + + CreateDiagnosedPublishers(); + + auto hardware_id = std::string("vn100-") + std::string(model_number_buffer) + + std::string(serial_number_buffer); + updater_.setHardwareID(hardware_id); +} + +void ImuVn100::Stream(bool async) { + // Pause the device first + VnEnsure(vn100_pauseAsyncOutputs(&imu_, true)); + + if (async) { + VnEnsure(vn100_setAsynchronousDataOutputType(&imu_, VNASYNC_OFF, true)); + + if (binary_output_) { + // Set the binary output data type and data rate + VnEnsure(vn100_setBinaryOutput1Configuration( + &imu_, BINARY_ASYNC_MODE_SERIAL_1, kBaseImuRate / imu_rate_, + BG1_QTN | BG1_IMU | BG1_MAG_PRES | BG1_SYNC_IN_CNT, + // BG1_IMU, + BG3_NONE, BG5_NONE, true)); + } else { + // Set the ASCII output data type and data rate + // ROS_INFO("Configure the output data type and frequency (id: 6 & 7)"); + VnEnsure(vn100_setAsynchronousDataOutputType(&imu_, VNASYNC_VNIMU, true)); + } + + // Add a callback function for new data event + VnEnsure(vn100_registerAsyncDataReceivedListener(&imu_, &AsyncListener)); + + ROS_INFO("Setting IMU rate to %d", imu_rate_); + VnEnsure(vn100_setAsynchronousDataOutputFrequency(&imu_, imu_rate_, true)); + } else { + // Mute the stream + ROS_DEBUG("Mute the device"); + VnEnsure(vn100_setAsynchronousDataOutputType(&imu_, VNASYNC_OFF, true)); + // Remove the callback function for new data event + VnEnsure(vn100_unregisterAsyncDataReceivedListener(&imu_, &AsyncListener)); + } + + // Resume the device + VnEnsure(vn100_resumeAsyncOutputs(&imu_, true)); +} + +void ImuVn100::Resume(bool need_reply) { + vn100_resumeAsyncOutputs(&imu_, need_reply); +} + +void ImuVn100::Idle(bool need_reply) { + vn100_pauseAsyncOutputs(&imu_, need_reply); +} + +void ImuVn100::Disconnect() { + // TODO: why reset the device? + vn100_reset(&imu_); + vn100_disconnect(&imu_); +} + +void ImuVn100::PublishData(const VnDeviceCompositeData& data) { + sensor_msgs::Imu imu_msg; + imu_msg.header.stamp = ros::Time::now(); + imu_msg.header.frame_id = frame_id_; + + FillImuMessage(imu_msg, data, binary_output_); + pd_imu_.Publish(imu_msg); + + if (enable_mag_) { + sensor_msgs::MagneticField mag_msg; + mag_msg.header = imu_msg.header; + RosVector3FromVnVector3(mag_msg.magnetic_field, data.magnetic); + pd_mag_.Publish(mag_msg); + } + + if (enable_pres_) { + sensor_msgs::FluidPressure pres_msg; + pres_msg.header = imu_msg.header; + pres_msg.fluid_pressure = data.pressure; + pd_pres_.Publish(pres_msg); + } + + if (enable_temp_) { + sensor_msgs::Temperature temp_msg; + temp_msg.header = imu_msg.header; + temp_msg.temperature = data.temperature; + pd_temp_.Publish(temp_msg); + } + + sync_info_.Update(data.syncInCnt, imu_msg.header.stamp); + + updater_.update(); +} + +void VnEnsure(const VnErrorCode& error_code) { + if (error_code == VNERR_NO_ERROR) return; + + switch (error_code) { + case VNERR_UNKNOWN_ERROR: + throw std::runtime_error("VN: Unknown error"); + case VNERR_NOT_IMPLEMENTED: + throw std::runtime_error("VN: Not implemented"); + case VNERR_TIMEOUT: + ROS_WARN("Opertation time out"); + break; + case VNERR_SENSOR_INVALID_PARAMETER: + ROS_WARN("VN: Sensor invalid paramter"); + break; + case VNERR_INVALID_VALUE: + ROS_WARN("VN: Invalid value"); + break; + case VNERR_FILE_NOT_FOUND: + ROS_WARN("VN: File not found"); + break; + case VNERR_NOT_CONNECTED: + throw std::runtime_error("VN: not connected"); + case VNERR_PERMISSION_DENIED: + throw std::runtime_error("VN: Permission denied"); + default: + ROS_WARN("Unhandled error type"); + } +} + +void RosVector3FromVnVector3(geometry_msgs::Vector3& ros_vec3, + const VnVector3& vn_vec3) { + ros_vec3.x = vn_vec3.c0; + ros_vec3.y = vn_vec3.c1; + ros_vec3.z = vn_vec3.c2; +} + +void RosQuaternionFromVnQuaternion(geometry_msgs::Quaternion& ros_quat, + const VnQuaternion& vn_quat) { + ros_quat.x = vn_quat.x; + ros_quat.y = vn_quat.y; + ros_quat.z = vn_quat.z; + ros_quat.w = vn_quat.w; +} + +void FillImuMessage(sensor_msgs::Imu& imu_msg, + const VnDeviceCompositeData& data, bool binary_output) { + if (binary_output) { + RosQuaternionFromVnQuaternion(imu_msg.orientation, data.quaternion); + // NOTE: The IMU angular velocity and linear acceleration outputs are + // swapped. And also why are they different? + RosVector3FromVnVector3(imu_msg.angular_velocity, + data.accelerationUncompensated); + RosVector3FromVnVector3(imu_msg.linear_acceleration, + data.angularRateUncompensated); + } else { + RosVector3FromVnVector3(imu_msg.linear_acceleration, data.acceleration); + RosVector3FromVnVector3(imu_msg.angular_velocity, data.angularRate); + } +} + +} // namespace imu_vn_100 From e80e80bfeeb1f552253810761e946c602afd09fc Mon Sep 17 00:00:00 2001 From: Andre Phu-Van Nguyen Date: Fri, 26 Feb 2016 03:40:35 -0500 Subject: [PATCH 03/10] done init --- include/imu_vn_100/imu_vn_100.h | 6 +- src/imu_vn_100.cpp | 183 +++++++++++++++++++++++++++----- 2 files changed, 159 insertions(+), 30 deletions(-) diff --git a/include/imu_vn_100/imu_vn_100.h b/include/imu_vn_100/imu_vn_100.h index 1a8b282..4cecca7 100644 --- a/include/imu_vn_100/imu_vn_100.h +++ b/include/imu_vn_100/imu_vn_100.h @@ -27,6 +27,9 @@ #include #include +#include "vn/sensors/sensors.h" +#include "vn/protocol/uart/types.h" + namespace imu_vn_100 { namespace du = diagnostic_updater; @@ -105,12 +108,13 @@ class ImuVn100 { private: ros::NodeHandle pnh_; - //Vn100 imu_; + vn::sensors::VnSensor imu_; // Settings std::string port_; int baudrate_ = 921600; int imu_rate_ = kDefaultImuRate; + vn::protocol::uart::AsyncMode vn_serial_output_ = vn::protocol::uart::ASYNCMODE_PORT1; double imu_rate_double_ = kDefaultImuRate; std::string frame_id_; diff --git a/src/imu_vn_100.cpp b/src/imu_vn_100.cpp index 3b16ff0..caa8bbd 100644 --- a/src/imu_vn_100.cpp +++ b/src/imu_vn_100.cpp @@ -17,6 +17,7 @@ */ #include +#include "vn/sensors/sensors.h" #include "vn/math/math.h" #include "vn/math/conversions.h" #include "vn/math/kinematics.h" @@ -45,60 +46,184 @@ constexpr int ImuVn100::kDefaultSyncOutRate; void ImuVn100::SyncInfo::Update(const unsigned sync_count, const ros::Time& sync_time) { - if (rate <= 0) return; + if (rate <= 0) return; - if (count != sync_count) { - count = sync_count; - time = sync_time; - } + if (count != sync_count) { + count = sync_count; + time = sync_time; + } } bool ImuVn100::SyncInfo::SyncEnabled() const { return rate > 0; } void ImuVn100::SyncInfo::FixSyncRate() { - // Check the sync out rate - if (SyncEnabled()) { - if (ImuVn100::kBaseImuRate % rate != 0) { - rate = ImuVn100::kBaseImuRate / (ImuVn100::kBaseImuRate / rate); - ROS_INFO("Set SYNC_OUT_RATE to %d", rate); - } - skip_count = - (std::floor(ImuVn100::kBaseImuRate / static_cast(rate) + - 0.5f)) - - 1; - - if (pulse_width_us > 10000) { - ROS_INFO("Sync out pulse with is over 10ms. Reset to 1ms"); - pulse_width_us = 1000; + // Check the sync out rate + if (SyncEnabled()) { + if (ImuVn100::kBaseImuRate % rate != 0) { + rate = ImuVn100::kBaseImuRate / (ImuVn100::kBaseImuRate / rate); + ROS_INFO("Set SYNC_OUT_RATE to %d", rate); + } + skip_count = + (std::floor(ImuVn100::kBaseImuRate / static_cast(rate) + + 0.5f)) - + 1; + + if (pulse_width_us > 10000) { + ROS_INFO("Sync out pulse with is over 10ms. Reset to 1ms"); + pulse_width_us = 1000; + } + rate_double = rate; } - rate_double = rate; - } - ROS_INFO("Sync out rate: %d", rate); + ROS_INFO("Sync out rate: %d", rate); } -ImuVn100::ImuVn100(const ros::NodeHandle& pnh){ - +ImuVn100::ImuVn100(const ros::NodeHandle& pnh) + : pnh_(pnh), + port_(std::string("/dev/ttyUSB0")), + baudrate_(921600), + frame_id_(std::string("imu")) { + Initialize(); + //TODO pointer hack } -ImuVn100::~ImuVn100() { - Disconnect(); -} +ImuVn100::~ImuVn100() { Disconnect(); } void ImuVn100::FixImuRate() { + if (imu_rate_ <= 0) { + ROS_WARN("Imu rate %d is < 0. Set to %d", imu_rate_, kDefaultImuRate); + imu_rate_ = kDefaultImuRate; + } + if (kBaseImuRate % imu_rate_ != 0) { + int imu_rate_old = imu_rate_; + imu_rate_ = kBaseImuRate / (kBaseImuRate / imu_rate_old); + ROS_WARN("Imu rate %d cannot evenly decimate base rate %d, reset to %d", + imu_rate_old, kBaseImuRate, imu_rate_); + } } void ImuVn100::LoadParameters() { + pnh_.param("port", port_, std::string("/dev/ttyUSB0")); + pnh_.param("frame_id", frame_id_, pnh_.getNamespace()); + pnh_.param("baudrate", baudrate_, 115200); + pnh_.param("imu_rate", imu_rate_, kDefaultImuRate); + + pnh_.param("enable_mag", enable_mag_, true); + pnh_.param("enable_pres", enable_pres_, true); + pnh_.param("enable_temp", enable_temp_, true); + + pnh_.param("sync_rate", sync_info_.rate, kDefaultSyncOutRate); + pnh_.param("sync_pulse_width_us", sync_info_.pulse_width_us, 1000); + + pnh_.param("binary_output", binary_output_, true); + int vn_serial_output_tmp = 4; //init on invalid number + pnh_.param("vn_serial_output", vn_serial_output_tmp, 1); + switch(vn_serial_output_tmp){ + case 0: + vn_serial_output_ = vn::protocol::uart::ASYNCMODE_NONE; + break; + case 1: + vn_serial_output_ = vn::protocol::uart::ASYNCMODE_PORT1; + break; + case 2: + vn_serial_output_ = vn::protocol::uart::ASYNCMODE_PORT2; + break; + case 3: + vn_serial_output_ = vn::protocol::uart::ASYNCMODE_BOTH; + break; + default: + ROS_ERROR("Incorrect VN serial port chosen."); + break; + } + FixImuRate(); + sync_info_.FixSyncRate(); } void ImuVn100::CreateDiagnosedPublishers() { - + imu_rate_double_ = imu_rate_; + pd_imu_.Create(pnh_, "imu", updater_, imu_rate_double_); + if (enable_mag_) { + pd_mag_.Create(pnh_, "magnetic_field", updater_, + imu_rate_double_); + } + if (enable_pres_) { + pd_pres_.Create(pnh_, "fluid_pressure", updater_, + imu_rate_double_); + } + if (enable_temp_) { + pd_temp_.Create(pnh_, "temperature", updater_, + imu_rate_double_); + } } void ImuVn100::Initialize() { - + LoadParameters(); + + // Try initial opening + ROS_DEBUG("Connecting to device"); + imu_.connect(port_, 115200); + ros::Duration(0.5).sleep(); + ROS_INFO("Conencted to device at %s", port_.c_str()); + + unsigned int old_baudrate = imu_.readSerialBaudRate(); + ROS_INFO("Default serial baudrate: %u", old_baudrate); + + ROS_INFO("Set serial baudrate to %d", baudrate_); + imu_.writeSerialBaudRate(baudrate_, true); + + ROS_DEBUG("Disconneceting the device"); + imu_.disconnect(); + ros::Duration(0.5).sleep(); + + // Open with the desired baud rate + ROS_DEBUG("Reconnecting to device"); + imu_.connect(port_, baudrate_); + ros::Duration(0.5).sleep(); + ROS_INFO("Connected to device at %s", port_.c_str()); + + old_baudrate = imu_.readSerialBaudRate(); + ROS_INFO("New serial baudrate: %u", old_baudrate); + + imu_.unregisterAsyncPacketReceivedHandler(); + imu_.unregisterErrorPacketReceivedHandler(); + + ROS_INFO("Fetching device info."); + std::string model_num = imu_.readModelNumber(); + ROS_INFO("Model number: %s", model_num.c_str()); + unsigned int hardw_rev = imu_.readHardwareRevision(); + ROS_INFO("Hardware revision: %d", hardw_rev); + unsigned int serial_num = imu_.readSerialNumber(); + ROS_INFO("Serial number: %d", serial_num); + std::string firmw_rev = imu_.readFirmwareVersion(); + ROS_INFO("Firmware version: %s", firmw_rev.c_str()); + + if (sync_info_.SyncEnabled()) { + ROS_INFO("Set Synchronization Control Register."); + imu_.writeSynchronizationControl( + vn::protocol::uart::SYNCINMODE_COUNT, + vn::protocol::uart::SYNCINEDGE_RISING, + 0, + vn::protocol::uart::SYNCOUTMODE_ITEMSTART, + vn::protocol::uart::SYNCOUTPOLARITY_POSITIVE, + sync_info_.skip_count, + sync_info_.pulse_width_us * 1000, + true); + + if (!binary_output_) { + ROS_INFO("Set Communication Protocol Control Register (id:30)."); + imu_.writeCommunicationProtocolControl( + vn::protocol::uart::COUNTMODE_SYNCOUTCOUNTER, + vn::protocol::uart::STATUSMODE_OFF, + vn::protocol::uart::COUNTMODE_NONE, // SPI + vn::protocol::uart::STATUSMODE_OFF, // SPI + vn::protocol::uart::CHECKSUMMODE_CHECKSUM, // serial checksum is 8bit + vn::protocol::uart::CHECKSUMMODE_CHECKSUM, // SPI + vn::protocol::uart::ERRORMODE_SEND, + true); + } + } } void ImuVn100::Stream(bool async){ From 94c022ccb6e5b51dae33a4e4f339119be6c29a88 Mon Sep 17 00:00:00 2001 From: Andre Nguyen Date: Fri, 26 Feb 2016 16:30:05 -0500 Subject: [PATCH 04/10] make vn prog lib shared --- ext/vnproglib-1.1.0.115/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ext/vnproglib-1.1.0.115/CMakeLists.txt b/ext/vnproglib-1.1.0.115/CMakeLists.txt index 1666e36..523c0c2 100755 --- a/ext/vnproglib-1.1.0.115/CMakeLists.txt +++ b/ext/vnproglib-1.1.0.115/CMakeLists.txt @@ -171,7 +171,7 @@ endif() # LIBRARY_OUTPUT_DIRECTORY "../vnpy") # endif() #else() -add_library(vnproglib +add_library(vnproglib SHARED ${top_sources} ${math_sources} ${util_sources} From a3dfbe5ca57a6068a193e5df44c98dfd18d61ba4 Mon Sep 17 00:00:00 2001 From: Andre Nguyen Date: Fri, 26 Feb 2016 22:24:41 -0500 Subject: [PATCH 05/10] data publishing working, still need to do error handling and other things --- include/imu_vn_100/imu_vn_100.h | 2 +- launch/vn_100_cont.launch | 5 +- src/imu_vn_100.cpp | 225 ++++++++++++++++++++++++++++---- 3 files changed, 202 insertions(+), 30 deletions(-) diff --git a/include/imu_vn_100/imu_vn_100.h b/include/imu_vn_100/imu_vn_100.h index 4cecca7..bda6cd1 100644 --- a/include/imu_vn_100/imu_vn_100.h +++ b/include/imu_vn_100/imu_vn_100.h @@ -78,7 +78,7 @@ class ImuVn100 { void Stream(bool async = true); -// void PublishData(const VnDeviceCompositeData& data); + void PublishData(vn::protocol::uart::Packet& p); void RequestOnce(); diff --git a/launch/vn_100_cont.launch b/launch/vn_100_cont.launch index 40629a8..26f1078 100644 --- a/launch/vn_100_cont.launch +++ b/launch/vn_100_cont.launch @@ -5,11 +5,11 @@ - + - + @@ -25,6 +25,7 @@ + diff --git a/src/imu_vn_100.cpp b/src/imu_vn_100.cpp index caa8bbd..470d5f2 100644 --- a/src/imu_vn_100.cpp +++ b/src/imu_vn_100.cpp @@ -35,10 +35,38 @@ void RosVector3FromVnVector3(geometry_msgs::Vector3& ros_vec3, /** * @brief RosQuaternionFromVnQuaternion * @param ros_quat - * @param vn_quat + * @param vn_vec4 */ -void RosQuaternionFromVnQuaternion(geometry_msgs::Quaternion& ros_quat, - const vn::math::kinematics::quat& vn_quat); +void RosQuaternionFromVnVector4(geometry_msgs::Quaternion& ros_quat, + const vn::math::vec4f& vn_vec4); + +/** + * @brief FillImuMessage + * @param imu_msg + * @param p + * @param binary_output + */ +void FillImuMessage(sensor_msgs::Imu& imu_msg, + vn::protocol::uart::Packet& p, bool binary_output); + +/** + * @brief asciiOrBinaryAsyncMessageReceived + * @param userData + * @param p + * @param index + */ +void asciiOrBinaryAsyncMessageReceived(void* userData, + vn::protocol::uart::Packet& p, + size_t index); + +/** + * @brief errorMessageReceived + * @param userData + * @param p + * @param index + */ +void errorMessageReceived(void* userData, vn::protocol::uart::Packet& p, + size_t index); constexpr int ImuVn100::kBaseImuRate; constexpr int ImuVn100::kDefaultImuRate; @@ -146,36 +174,41 @@ void ImuVn100::CreateDiagnosedPublishers() { pd_imu_.Create(pnh_, "imu", updater_, imu_rate_double_); if (enable_mag_) { pd_mag_.Create(pnh_, "magnetic_field", updater_, - imu_rate_double_); + imu_rate_double_); } if (enable_pres_) { pd_pres_.Create(pnh_, "fluid_pressure", updater_, - imu_rate_double_); + imu_rate_double_); } if (enable_temp_) { pd_temp_.Create(pnh_, "temperature", updater_, - imu_rate_double_); + imu_rate_double_); } } void ImuVn100::Initialize() { LoadParameters(); - + unsigned int old_baudrate; // Try initial opening - ROS_DEBUG("Connecting to device"); - imu_.connect(port_, 115200); - ros::Duration(0.5).sleep(); - ROS_INFO("Conencted to device at %s", port_.c_str()); - - unsigned int old_baudrate = imu_.readSerialBaudRate(); - ROS_INFO("Default serial baudrate: %u", old_baudrate); - - ROS_INFO("Set serial baudrate to %d", baudrate_); - imu_.writeSerialBaudRate(baudrate_, true); - - ROS_DEBUG("Disconneceting the device"); - imu_.disconnect(); - ros::Duration(0.5).sleep(); + try{ + ROS_INFO("Connecting to device"); + imu_.connect(port_, 115200); + ros::Duration(1).sleep(); + ROS_INFO("Connected to device at %s", port_.c_str()); + + old_baudrate = imu_.readSerialBaudRate(); + ROS_INFO("Default serial baudrate: %u", old_baudrate); + + ROS_INFO("Set serial baudrate to %d", baudrate_); + imu_.writeSerialBaudRate(baudrate_, true); + + ROS_DEBUG("Disconnecting the device"); + imu_.disconnect(); + ros::Duration(0.5).sleep(); + } catch (std::exception except){ + ROS_DEBUG("Failed to open device with default baudrate with exception: %s", + except.what()); + } // Open with the desired baud rate ROS_DEBUG("Reconnecting to device"); @@ -186,9 +219,6 @@ void ImuVn100::Initialize() { old_baudrate = imu_.readSerialBaudRate(); ROS_INFO("New serial baudrate: %u", old_baudrate); - imu_.unregisterAsyncPacketReceivedHandler(); - imu_.unregisterErrorPacketReceivedHandler(); - ROS_INFO("Fetching device info."); std::string model_num = imu_.readModelNumber(); ROS_INFO("Model number: %s", model_num.c_str()); @@ -224,10 +254,58 @@ void ImuVn100::Initialize() { true); } } + + CreateDiagnosedPublishers(); + + auto hardware_id = std::string("vn100-") + model_num + + std::to_string(serial_num); + updater_.setHardwareID(hardware_id); } void ImuVn100::Stream(bool async){ + // TODO There isn't a pause function in the new lib, what do here? + using namespace vn::protocol::uart; + + if (async) + { + imu_.writeAsyncDataOutputType(VNOFF, true); + + if (binary_output_) { + // Set the binary output data type and data rate + vn::sensors::BinaryOutputRegister bor( + vn_serial_output_, + kBaseImuRate / imu_rate_, + (COMMONGROUP_QUATERNION | COMMONGROUP_IMU | + COMMONGROUP_MAGPRES | COMMONGROUP_SYNCINCNT), + TIMEGROUP_NONE, + IMUGROUP_NONE, + GPSGROUP_NONE, + ATTITUDEGROUP_NONE, + INSGROUP_NONE); + + imu_.writeBinaryOutput1(bor, true); + } else { + // Set the ASCII output data type and data rate + imu_.writeAsyncDataOutputType(VNIMU, true); + } + // add a callback function for new data event + imu_.registerAsyncPacketReceivedHandler(this, asciiOrBinaryAsyncMessageReceived); + + ROS_INFO("Setting IMU rate to %d", imu_rate_); + imu_.writeAsyncDataOutputFrequency(imu_rate_, true); + } else { + // Mute the stream + ROS_DEBUG("Mute the device"); + imu_.writeAsyncDataOutputType(VNOFF, true); + // Remove the callback function for new data event + try { + imu_.unregisterAsyncPacketReceivedHandler(); + } catch (std::exception except) { + ROS_WARN("Unable to unregister async packet handler: %s", + except.what()); + } + } } void ImuVn100::Resume(bool need_reply) { @@ -241,9 +319,102 @@ void ImuVn100::Idle(bool need_reply) { void ImuVn100::Disconnect() { } -/* -void ImuVn100::PublishData(const VnDeviceCompositeData& data) { -}*/ +void ImuVn100::PublishData(vn::protocol::uart::Packet& p) { + sensor_msgs::Imu imu_msg; + imu_msg.header.stamp = ros::Time::now(); + imu_msg.header.frame_id = frame_id_; + + FillImuMessage(imu_msg, p, binary_output_); + pd_imu_.Publish(imu_msg); + if (enable_mag_) { + sensor_msgs::MagneticField mag_msg; + mag_msg.header = imu_msg.header; + vn::math::vec3f magnetometer = p.extractVec3f();// COMMONGROUP_MAGPRES + RosVector3FromVnVector3(mag_msg.magnetic_field, magnetometer); + pd_mag_.Publish(mag_msg); + } + + if (enable_temp_) { + sensor_msgs::Temperature temp_msg; + temp_msg.header = imu_msg.header; + float temperature = p.extractFloat(); // COMMONGROUP_MAGPRES + temp_msg.temperature = temperature; + pd_temp_.Publish(temp_msg); + } + + if (enable_pres_) { + sensor_msgs::FluidPressure pres_msg; + pres_msg.header = imu_msg.header; + float pressure = p.extractFloat(); // COMMONGROUP_MAGPRES + pres_msg.fluid_pressure = pressure; + pd_pres_.Publish(pres_msg); + } + + unsigned int syncInCnt = p.extractUint32(); + sync_info_.Update(syncInCnt, imu_msg.header.stamp); + + updater_.update(); +} + +void RosVector3FromVnVector3(geometry_msgs::Vector3& ros_vec3, + const vn::math::vec3f& vn_vec3){ + ros_vec3.x = vn_vec3[0]; + ros_vec3.y = vn_vec3[1]; + ros_vec3.z = vn_vec3[2]; } + +void RosQuaternionFromVnVector4(geometry_msgs::Quaternion& ros_quat, + const vn::math::vec4f& vn_vec4) { + ros_quat.x = vn_vec4[0]; //see quaternion application note + ros_quat.y = vn_vec4[1]; + ros_quat.z = vn_vec4[2]; + ros_quat.w = vn_vec4[3]; +} + + +void FillImuMessage(sensor_msgs::Imu& imu_msg, + vn::protocol::uart::Packet& p, bool binary_output){ + if (binary_output) { + // Note: With this library, we are responsible for extracting the data + // in the appropriate order! Need to refer to manual and to how we + // configured the common output group + vn::math::vec4f quaternion = p.extractVec4f(); // COMMONGROUP_QUATERNION + // NOTE: The IMU angular velocity and linear acceleration outputs are + // swapped. And also why are they different? + vn::math::vec3f linear_accel = p.extractVec3f();// COMMONGROUP_IMU + vn::math::vec3f angular_rate = p.extractVec3f();// COMMONGROUP_IMU + + RosQuaternionFromVnVector4(imu_msg.orientation, quaternion); + RosVector3FromVnVector3(imu_msg.angular_velocity, angular_rate); + RosVector3FromVnVector3(imu_msg.linear_acceleration, linear_accel); + } else { + // TODO + } +} + +void asciiOrBinaryAsyncMessageReceived(void* userData, vn::protocol::uart::Packet& p, size_t index){ + using namespace vn::protocol::uart; + ImuVn100* imu = (ImuVn100*) userData; + if (!p.isCompatible( + (COMMONGROUP_QUATERNION | COMMONGROUP_IMU | + COMMONGROUP_MAGPRES | COMMONGROUP_SYNCINCNT), + TIMEGROUP_NONE, + IMUGROUP_NONE, + GPSGROUP_NONE, + ATTITUDEGROUP_NONE, + INSGROUP_NONE)) + // Not the type of binary packet we are expecting. + return; + + imu->PublishData(p); +} + +void errorMessageReceived(void* userData, vn::protocol::uart::Packet& p, + size_t index) { + ImuVn100* imu = (ImuVn100*) userData; +} + +} // end namespace + From fa1e814b73219c42ffb410032047c7f0ceafb38b Mon Sep 17 00:00:00 2001 From: Andre Nguyen Date: Sat, 27 Feb 2016 19:10:45 -0500 Subject: [PATCH 06/10] add error handling --- src/imu_vn_100.cpp | 51 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 51 insertions(+) diff --git a/src/imu_vn_100.cpp b/src/imu_vn_100.cpp index 470d5f2..b07d342 100644 --- a/src/imu_vn_100.cpp +++ b/src/imu_vn_100.cpp @@ -413,7 +413,58 @@ void asciiOrBinaryAsyncMessageReceived(void* userData, vn::protocol::uart::Packe void errorMessageReceived(void* userData, vn::protocol::uart::Packet& p, size_t index) { + using vn::protocol::uart::SensorError; ImuVn100* imu = (ImuVn100*) userData; + SensorError se = p.parseError(); + + if (se == 0) return; + + switch (se) { + case SensorError::ERR_HARD_FAULT: + ROS_ERROR("VN: Hard fault. Processor will force restart."); + break; + case SensorError::ERR_SERIAL_BUFFER_OVERFLOW: ///< Serial buffer overflow. + // We tried sending some kind of crazy long command which is impossible + // Throw because the developper shouldn't do this. + throw std::runtime_error("VN: Serial buffer overflow."); + break; + case SensorError::ERR_INVALID_CHECKSUM: ///< Invalid checksum. + ROS_WARN("VN: Invalid checksum on packet %s", std::to_string(index).c_str()); + break; + case SensorError::ERR_INVALID_COMMAND: ///< Invalid command. + ROS_WARN("VN: Invalid command on packet %s", std::to_string(index).c_str()); + break; + case SensorError::ERR_NOT_ENOUGH_PARAMETERS: ///< Not enough parameters. + ROS_WARN("VN: Not enough parameters."); + break; + case SensorError::ERR_TOO_MANY_PARAMETERS: ///< Too many parameters. + ROS_WARN("VN: Too many parameters."); + break; + case SensorError::ERR_INVALID_PARAMETER: ///< Invalid parameter. + ROS_WARN("VN: Invalid parameter."); + break; + case SensorError::ERR_INVALID_REGISTER: ///< Invalid register. + ROS_WARN("VN: Invalid register."); + break; + case SensorError::ERR_UNAUTHORIZED_ACCESS: ///< Unauthorized access. + ROS_WARN("VN: Unauthorized access to a register."); + break; + case SensorError::ERR_WATCHDOG_RESET: ///< Watchdog reset + ROS_WARN("VN: Watchdog reset has occured. VN should have restarted within 50 ms."); + break; + case SensorError::ERR_OUTPUT_BUFFER_OVERFLOW: ///< Output buffer overflow. + ROS_WARN("VN: Output buffer overflow."); + break; + case SensorError::ERR_INSUFFICIENT_BAUD_RATE: ///< Insufficient baud rate. + ROS_WARN("VN: Insufficient baud rate for requested async data output and rate."); + break; + case SensorError::ERR_ERROR_BUFFER_OVERFLOW: ///< Error buffer overflow. + ROS_WARN("VN: System error buffer overflow."); + break; + default: + throw std::runtime_error("VN: Unknown error code " + std::to_string(se)); + break; + } } } // end namespace From f04082faa8c6a07585adc589d89020518e378f7f Mon Sep 17 00:00:00 2001 From: Andre Nguyen Date: Sat, 27 Feb 2016 19:23:55 -0500 Subject: [PATCH 07/10] add CRC/checksum verification --- src/imu_vn_100.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/imu_vn_100.cpp b/src/imu_vn_100.cpp index b07d342..db034c6 100644 --- a/src/imu_vn_100.cpp +++ b/src/imu_vn_100.cpp @@ -408,6 +408,11 @@ void asciiOrBinaryAsyncMessageReceived(void* userData, vn::protocol::uart::Packe // Not the type of binary packet we are expecting. return; + if (!p.isValid()) { + ROS_WARN("Vn: Invalid packet received. CRC or checksum failed."); + return; + } + imu->PublishData(p); } From ab860545017496ea09530ab966fa378674d588b2 Mon Sep 17 00:00:00 2001 From: Andre Nguyen Date: Sat, 27 Feb 2016 20:26:12 -0500 Subject: [PATCH 08/10] clang-format, follow google style --- src/imu_vn_100.cpp | 643 ++++++++++++++++++++++----------------------- 1 file changed, 315 insertions(+), 328 deletions(-) diff --git a/src/imu_vn_100.cpp b/src/imu_vn_100.cpp index db034c6..a18808e 100644 --- a/src/imu_vn_100.cpp +++ b/src/imu_vn_100.cpp @@ -46,8 +46,8 @@ void RosQuaternionFromVnVector4(geometry_msgs::Quaternion& ros_quat, * @param p * @param binary_output */ -void FillImuMessage(sensor_msgs::Imu& imu_msg, - vn::protocol::uart::Packet& p, bool binary_output); +void FillImuMessage(sensor_msgs::Imu& imu_msg, vn::protocol::uart::Packet& p, + bool binary_output); /** * @brief asciiOrBinaryAsyncMessageReceived @@ -74,403 +74,390 @@ constexpr int ImuVn100::kDefaultSyncOutRate; void ImuVn100::SyncInfo::Update(const unsigned sync_count, const ros::Time& sync_time) { - if (rate <= 0) return; + if (rate <= 0) return; - if (count != sync_count) { - count = sync_count; - time = sync_time; - } + if (count != sync_count) { + count = sync_count; + time = sync_time; + } } bool ImuVn100::SyncInfo::SyncEnabled() const { return rate > 0; } void ImuVn100::SyncInfo::FixSyncRate() { - // Check the sync out rate - if (SyncEnabled()) { - if (ImuVn100::kBaseImuRate % rate != 0) { - rate = ImuVn100::kBaseImuRate / (ImuVn100::kBaseImuRate / rate); - ROS_INFO("Set SYNC_OUT_RATE to %d", rate); - } - skip_count = - (std::floor(ImuVn100::kBaseImuRate / static_cast(rate) + - 0.5f)) - - 1; - - if (pulse_width_us > 10000) { - ROS_INFO("Sync out pulse with is over 10ms. Reset to 1ms"); - pulse_width_us = 1000; - } - rate_double = rate; + // Check the sync out rate + if (SyncEnabled()) { + if (ImuVn100::kBaseImuRate % rate != 0) { + rate = ImuVn100::kBaseImuRate / (ImuVn100::kBaseImuRate / rate); + ROS_INFO("Set SYNC_OUT_RATE to %d", rate); + } + skip_count = + (std::floor(ImuVn100::kBaseImuRate / static_cast(rate) + + 0.5f)) - + 1; + + if (pulse_width_us > 10000) { + ROS_INFO("Sync out pulse with is over 10ms. Reset to 1ms"); + pulse_width_us = 1000; } + rate_double = rate; + } - ROS_INFO("Sync out rate: %d", rate); + ROS_INFO("Sync out rate: %d", rate); } ImuVn100::ImuVn100(const ros::NodeHandle& pnh) - : pnh_(pnh), + : pnh_(pnh), port_(std::string("/dev/ttyUSB0")), baudrate_(921600), frame_id_(std::string("imu")) { - Initialize(); - //TODO pointer hack + Initialize(); + // TODO pointer hack } ImuVn100::~ImuVn100() { Disconnect(); } void ImuVn100::FixImuRate() { - if (imu_rate_ <= 0) { - ROS_WARN("Imu rate %d is < 0. Set to %d", imu_rate_, kDefaultImuRate); - imu_rate_ = kDefaultImuRate; - } - - if (kBaseImuRate % imu_rate_ != 0) { - int imu_rate_old = imu_rate_; - imu_rate_ = kBaseImuRate / (kBaseImuRate / imu_rate_old); - ROS_WARN("Imu rate %d cannot evenly decimate base rate %d, reset to %d", - imu_rate_old, kBaseImuRate, imu_rate_); - } + if (imu_rate_ <= 0) { + ROS_WARN("Imu rate %d is < 0. Set to %d", imu_rate_, kDefaultImuRate); + imu_rate_ = kDefaultImuRate; + } + + if (kBaseImuRate % imu_rate_ != 0) { + int imu_rate_old = imu_rate_; + imu_rate_ = kBaseImuRate / (kBaseImuRate / imu_rate_old); + ROS_WARN("Imu rate %d cannot evenly decimate base rate %d, reset to %d", + imu_rate_old, kBaseImuRate, imu_rate_); + } } void ImuVn100::LoadParameters() { - pnh_.param("port", port_, std::string("/dev/ttyUSB0")); - pnh_.param("frame_id", frame_id_, pnh_.getNamespace()); - pnh_.param("baudrate", baudrate_, 115200); - pnh_.param("imu_rate", imu_rate_, kDefaultImuRate); - - pnh_.param("enable_mag", enable_mag_, true); - pnh_.param("enable_pres", enable_pres_, true); - pnh_.param("enable_temp", enable_temp_, true); - - pnh_.param("sync_rate", sync_info_.rate, kDefaultSyncOutRate); - pnh_.param("sync_pulse_width_us", sync_info_.pulse_width_us, 1000); - - pnh_.param("binary_output", binary_output_, true); - int vn_serial_output_tmp = 4; //init on invalid number - pnh_.param("vn_serial_output", vn_serial_output_tmp, 1); - switch(vn_serial_output_tmp){ + pnh_.param("port", port_, std::string("/dev/ttyUSB0")); + pnh_.param("frame_id", frame_id_, pnh_.getNamespace()); + pnh_.param("baudrate", baudrate_, 115200); + pnh_.param("imu_rate", imu_rate_, kDefaultImuRate); + + pnh_.param("enable_mag", enable_mag_, true); + pnh_.param("enable_pres", enable_pres_, true); + pnh_.param("enable_temp", enable_temp_, true); + + pnh_.param("sync_rate", sync_info_.rate, kDefaultSyncOutRate); + pnh_.param("sync_pulse_width_us", sync_info_.pulse_width_us, 1000); + + pnh_.param("binary_output", binary_output_, true); + int vn_serial_output_tmp = 4; // init on invalid number + pnh_.param("vn_serial_output", vn_serial_output_tmp, 1); + switch (vn_serial_output_tmp) { case 0: - vn_serial_output_ = vn::protocol::uart::ASYNCMODE_NONE; - break; + vn_serial_output_ = vn::protocol::uart::ASYNCMODE_NONE; + break; case 1: - vn_serial_output_ = vn::protocol::uart::ASYNCMODE_PORT1; - break; + vn_serial_output_ = vn::protocol::uart::ASYNCMODE_PORT1; + break; case 2: - vn_serial_output_ = vn::protocol::uart::ASYNCMODE_PORT2; - break; + vn_serial_output_ = vn::protocol::uart::ASYNCMODE_PORT2; + break; case 3: - vn_serial_output_ = vn::protocol::uart::ASYNCMODE_BOTH; - break; + vn_serial_output_ = vn::protocol::uart::ASYNCMODE_BOTH; + break; default: - ROS_ERROR("Incorrect VN serial port chosen."); - break; - } + ROS_ERROR("Incorrect VN serial port chosen."); + break; + } - FixImuRate(); - sync_info_.FixSyncRate(); + FixImuRate(); + sync_info_.FixSyncRate(); } void ImuVn100::CreateDiagnosedPublishers() { - imu_rate_double_ = imu_rate_; - pd_imu_.Create(pnh_, "imu", updater_, imu_rate_double_); - if (enable_mag_) { - pd_mag_.Create(pnh_, "magnetic_field", updater_, - imu_rate_double_); - } - if (enable_pres_) { - pd_pres_.Create(pnh_, "fluid_pressure", updater_, - imu_rate_double_); - } - if (enable_temp_) { - pd_temp_.Create(pnh_, "temperature", updater_, - imu_rate_double_); - } + imu_rate_double_ = imu_rate_; + pd_imu_.Create(pnh_, "imu", updater_, imu_rate_double_); + if (enable_mag_) { + pd_mag_.Create(pnh_, "magnetic_field", updater_, + imu_rate_double_); + } + if (enable_pres_) { + pd_pres_.Create(pnh_, "fluid_pressure", + updater_, imu_rate_double_); + } + if (enable_temp_) { + pd_temp_.Create(pnh_, "temperature", updater_, + imu_rate_double_); + } } void ImuVn100::Initialize() { - LoadParameters(); - unsigned int old_baudrate; - // Try initial opening - try{ - ROS_INFO("Connecting to device"); - imu_.connect(port_, 115200); - ros::Duration(1).sleep(); - ROS_INFO("Connected to device at %s", port_.c_str()); - - old_baudrate = imu_.readSerialBaudRate(); - ROS_INFO("Default serial baudrate: %u", old_baudrate); - - ROS_INFO("Set serial baudrate to %d", baudrate_); - imu_.writeSerialBaudRate(baudrate_, true); - - ROS_DEBUG("Disconnecting the device"); - imu_.disconnect(); - ros::Duration(0.5).sleep(); - } catch (std::exception except){ - ROS_DEBUG("Failed to open device with default baudrate with exception: %s", - except.what()); - } - - // Open with the desired baud rate - ROS_DEBUG("Reconnecting to device"); - imu_.connect(port_, baudrate_); - ros::Duration(0.5).sleep(); + LoadParameters(); + unsigned int old_baudrate; + // Try initial opening + try { + ROS_INFO("Connecting to device"); + imu_.connect(port_, 115200); + ros::Duration(1).sleep(); ROS_INFO("Connected to device at %s", port_.c_str()); old_baudrate = imu_.readSerialBaudRate(); - ROS_INFO("New serial baudrate: %u", old_baudrate); - - ROS_INFO("Fetching device info."); - std::string model_num = imu_.readModelNumber(); - ROS_INFO("Model number: %s", model_num.c_str()); - unsigned int hardw_rev = imu_.readHardwareRevision(); - ROS_INFO("Hardware revision: %d", hardw_rev); - unsigned int serial_num = imu_.readSerialNumber(); - ROS_INFO("Serial number: %d", serial_num); - std::string firmw_rev = imu_.readFirmwareVersion(); - ROS_INFO("Firmware version: %s", firmw_rev.c_str()); - - if (sync_info_.SyncEnabled()) { - ROS_INFO("Set Synchronization Control Register."); - imu_.writeSynchronizationControl( - vn::protocol::uart::SYNCINMODE_COUNT, - vn::protocol::uart::SYNCINEDGE_RISING, - 0, - vn::protocol::uart::SYNCOUTMODE_ITEMSTART, - vn::protocol::uart::SYNCOUTPOLARITY_POSITIVE, - sync_info_.skip_count, - sync_info_.pulse_width_us * 1000, - true); - - if (!binary_output_) { - ROS_INFO("Set Communication Protocol Control Register (id:30)."); - imu_.writeCommunicationProtocolControl( - vn::protocol::uart::COUNTMODE_SYNCOUTCOUNTER, - vn::protocol::uart::STATUSMODE_OFF, - vn::protocol::uart::COUNTMODE_NONE, // SPI - vn::protocol::uart::STATUSMODE_OFF, // SPI - vn::protocol::uart::CHECKSUMMODE_CHECKSUM, // serial checksum is 8bit - vn::protocol::uart::CHECKSUMMODE_CHECKSUM, // SPI - vn::protocol::uart::ERRORMODE_SEND, - true); - } - } + ROS_INFO("Default serial baudrate: %u", old_baudrate); - CreateDiagnosedPublishers(); + ROS_INFO("Set serial baudrate to %d", baudrate_); + imu_.writeSerialBaudRate(baudrate_, true); - auto hardware_id = std::string("vn100-") + model_num + - std::to_string(serial_num); - updater_.setHardwareID(hardware_id); -} - -void ImuVn100::Stream(bool async){ - // TODO There isn't a pause function in the new lib, what do here? - using namespace vn::protocol::uart; - - if (async) - { - imu_.writeAsyncDataOutputType(VNOFF, true); - - if (binary_output_) { - // Set the binary output data type and data rate - vn::sensors::BinaryOutputRegister bor( - vn_serial_output_, - kBaseImuRate / imu_rate_, - (COMMONGROUP_QUATERNION | COMMONGROUP_IMU | - COMMONGROUP_MAGPRES | COMMONGROUP_SYNCINCNT), - TIMEGROUP_NONE, - IMUGROUP_NONE, - GPSGROUP_NONE, - ATTITUDEGROUP_NONE, - INSGROUP_NONE); - - imu_.writeBinaryOutput1(bor, true); - } else { - // Set the ASCII output data type and data rate - imu_.writeAsyncDataOutputType(VNIMU, true); - } - - // add a callback function for new data event - imu_.registerAsyncPacketReceivedHandler(this, asciiOrBinaryAsyncMessageReceived); - - ROS_INFO("Setting IMU rate to %d", imu_rate_); - imu_.writeAsyncDataOutputFrequency(imu_rate_, true); - } else { - // Mute the stream - ROS_DEBUG("Mute the device"); - imu_.writeAsyncDataOutputType(VNOFF, true); - // Remove the callback function for new data event - try { - imu_.unregisterAsyncPacketReceivedHandler(); - } catch (std::exception except) { - ROS_WARN("Unable to unregister async packet handler: %s", - except.what()); - } + ROS_DEBUG("Disconnecting the device"); + imu_.disconnect(); + ros::Duration(0.5).sleep(); + } catch (std::exception except) { + ROS_DEBUG("Failed to open device with default baudrate with exception: %s", + except.what()); + } + + // Open with the desired baud rate + ROS_DEBUG("Reconnecting to device"); + imu_.connect(port_, baudrate_); + ros::Duration(0.5).sleep(); + ROS_INFO("Connected to device at %s", port_.c_str()); + + old_baudrate = imu_.readSerialBaudRate(); + ROS_INFO("New serial baudrate: %u", old_baudrate); + + ROS_INFO("Fetching device info."); + std::string model_num = imu_.readModelNumber(); + ROS_INFO("Model number: %s", model_num.c_str()); + unsigned int hardw_rev = imu_.readHardwareRevision(); + ROS_INFO("Hardware revision: %d", hardw_rev); + unsigned int serial_num = imu_.readSerialNumber(); + ROS_INFO("Serial number: %d", serial_num); + std::string firmw_rev = imu_.readFirmwareVersion(); + ROS_INFO("Firmware version: %s", firmw_rev.c_str()); + + if (sync_info_.SyncEnabled()) { + ROS_INFO("Set Synchronization Control Register."); + imu_.writeSynchronizationControl( + vn::protocol::uart::SYNCINMODE_COUNT, + vn::protocol::uart::SYNCINEDGE_RISING, 0, + vn::protocol::uart::SYNCOUTMODE_ITEMSTART, + vn::protocol::uart::SYNCOUTPOLARITY_POSITIVE, sync_info_.skip_count, + sync_info_.pulse_width_us * 1000, true); + + if (!binary_output_) { + ROS_INFO("Set Communication Protocol Control Register (id:30)."); + imu_.writeCommunicationProtocolControl( + vn::protocol::uart::COUNTMODE_SYNCOUTCOUNTER, + vn::protocol::uart::STATUSMODE_OFF, + vn::protocol::uart::COUNTMODE_NONE, // SPI + vn::protocol::uart::STATUSMODE_OFF, // SPI + vn::protocol::uart::CHECKSUMMODE_CHECKSUM, // serial checksum is 8bit + vn::protocol::uart::CHECKSUMMODE_CHECKSUM, // SPI + vn::protocol::uart::ERRORMODE_SEND, true); } -} + } -void ImuVn100::Resume(bool need_reply) { + CreateDiagnosedPublishers(); + auto hardware_id = + std::string("vn100-") + model_num + std::to_string(serial_num); + updater_.setHardwareID(hardware_id); } -void ImuVn100::Idle(bool need_reply) { +void ImuVn100::Stream(bool async) { + // TODO There isn't a pause function in the new lib, what do here? + using namespace vn::protocol::uart; -} - -void ImuVn100::Disconnect() { + if (async) { + imu_.writeAsyncDataOutputType(VNOFF, true); -} + if (binary_output_) { + // Set the binary output data type and data rate + vn::sensors::BinaryOutputRegister bor( + vn_serial_output_, kBaseImuRate / imu_rate_, + (COMMONGROUP_QUATERNION | COMMONGROUP_IMU | COMMONGROUP_MAGPRES | + COMMONGROUP_SYNCINCNT), + TIMEGROUP_NONE, IMUGROUP_NONE, GPSGROUP_NONE, ATTITUDEGROUP_NONE, + INSGROUP_NONE); -void ImuVn100::PublishData(vn::protocol::uart::Packet& p) { - sensor_msgs::Imu imu_msg; - imu_msg.header.stamp = ros::Time::now(); - imu_msg.header.frame_id = frame_id_; - - FillImuMessage(imu_msg, p, binary_output_); - pd_imu_.Publish(imu_msg); - - if (enable_mag_) { - sensor_msgs::MagneticField mag_msg; - mag_msg.header = imu_msg.header; - vn::math::vec3f magnetometer = p.extractVec3f();// COMMONGROUP_MAGPRES - RosVector3FromVnVector3(mag_msg.magnetic_field, magnetometer); - pd_mag_.Publish(mag_msg); + imu_.writeBinaryOutput1(bor, true); + } else { + // Set the ASCII output data type and data rate + imu_.writeAsyncDataOutputType(VNIMU, true); } - if (enable_temp_) { - sensor_msgs::Temperature temp_msg; - temp_msg.header = imu_msg.header; - float temperature = p.extractFloat(); // COMMONGROUP_MAGPRES - temp_msg.temperature = temperature; - pd_temp_.Publish(temp_msg); + // add a callback function for new data event + imu_.registerAsyncPacketReceivedHandler(this, + asciiOrBinaryAsyncMessageReceived); + + ROS_INFO("Setting IMU rate to %d", imu_rate_); + imu_.writeAsyncDataOutputFrequency(imu_rate_, true); + } else { + // Mute the stream + ROS_DEBUG("Mute the device"); + imu_.writeAsyncDataOutputType(VNOFF, true); + // Remove the callback function for new data event + try { + imu_.unregisterAsyncPacketReceivedHandler(); + } catch (std::exception except) { + ROS_WARN("Unable to unregister async packet handler: %s", except.what()); } + } +} - if (enable_pres_) { - sensor_msgs::FluidPressure pres_msg; - pres_msg.header = imu_msg.header; - float pressure = p.extractFloat(); // COMMONGROUP_MAGPRES - pres_msg.fluid_pressure = pressure; - pd_pres_.Publish(pres_msg); - } +void ImuVn100::Resume(bool need_reply) {} - unsigned int syncInCnt = p.extractUint32(); - sync_info_.Update(syncInCnt, imu_msg.header.stamp); +void ImuVn100::Idle(bool need_reply) {} - updater_.update(); +void ImuVn100::Disconnect() {} + +void ImuVn100::PublishData(vn::protocol::uart::Packet& p) { + sensor_msgs::Imu imu_msg; + imu_msg.header.stamp = ros::Time::now(); + imu_msg.header.frame_id = frame_id_; + + FillImuMessage(imu_msg, p, binary_output_); + pd_imu_.Publish(imu_msg); + + if (enable_mag_) { + sensor_msgs::MagneticField mag_msg; + mag_msg.header = imu_msg.header; + vn::math::vec3f magnetometer = p.extractVec3f(); // COMMONGROUP_MAGPRES + RosVector3FromVnVector3(mag_msg.magnetic_field, magnetometer); + pd_mag_.Publish(mag_msg); + } + + if (enable_temp_) { + sensor_msgs::Temperature temp_msg; + temp_msg.header = imu_msg.header; + float temperature = p.extractFloat(); // COMMONGROUP_MAGPRES + temp_msg.temperature = temperature; + pd_temp_.Publish(temp_msg); + } + + if (enable_pres_) { + sensor_msgs::FluidPressure pres_msg; + pres_msg.header = imu_msg.header; + float pressure = p.extractFloat(); // COMMONGROUP_MAGPRES + pres_msg.fluid_pressure = pressure; + pd_pres_.Publish(pres_msg); + } + + unsigned int syncInCnt = p.extractUint32(); + sync_info_.Update(syncInCnt, imu_msg.header.stamp); + + updater_.update(); } void RosVector3FromVnVector3(geometry_msgs::Vector3& ros_vec3, - const vn::math::vec3f& vn_vec3){ - ros_vec3.x = vn_vec3[0]; - ros_vec3.y = vn_vec3[1]; - ros_vec3.z = vn_vec3[2]; + const vn::math::vec3f& vn_vec3) { + ros_vec3.x = vn_vec3[0]; + ros_vec3.y = vn_vec3[1]; + ros_vec3.z = vn_vec3[2]; } void RosQuaternionFromVnVector4(geometry_msgs::Quaternion& ros_quat, const vn::math::vec4f& vn_vec4) { - ros_quat.x = vn_vec4[0]; //see quaternion application note - ros_quat.y = vn_vec4[1]; - ros_quat.z = vn_vec4[2]; - ros_quat.w = vn_vec4[3]; + ros_quat.x = vn_vec4[0]; // see quaternion application note + ros_quat.y = vn_vec4[1]; + ros_quat.z = vn_vec4[2]; + ros_quat.w = vn_vec4[3]; } - -void FillImuMessage(sensor_msgs::Imu& imu_msg, - vn::protocol::uart::Packet& p, bool binary_output){ - if (binary_output) { - // Note: With this library, we are responsible for extracting the data - // in the appropriate order! Need to refer to manual and to how we - // configured the common output group - vn::math::vec4f quaternion = p.extractVec4f(); // COMMONGROUP_QUATERNION - // NOTE: The IMU angular velocity and linear acceleration outputs are - // swapped. And also why are they different? - vn::math::vec3f linear_accel = p.extractVec3f();// COMMONGROUP_IMU - vn::math::vec3f angular_rate = p.extractVec3f();// COMMONGROUP_IMU - - RosQuaternionFromVnVector4(imu_msg.orientation, quaternion); - RosVector3FromVnVector3(imu_msg.angular_velocity, angular_rate); - RosVector3FromVnVector3(imu_msg.linear_acceleration, linear_accel); - } else { - // TODO - } +void FillImuMessage(sensor_msgs::Imu& imu_msg, vn::protocol::uart::Packet& p, + bool binary_output) { + if (binary_output) { + // Note: With this library, we are responsible for extracting the data + // in the appropriate order! Need to refer to manual and to how we + // configured the common output group + vn::math::vec4f quaternion = p.extractVec4f(); // COMMONGROUP_QUATERNION + // NOTE: The IMU angular velocity and linear acceleration outputs are + // swapped. And also why are they different? + vn::math::vec3f linear_accel = p.extractVec3f(); // COMMONGROUP_IMU + vn::math::vec3f angular_rate = p.extractVec3f(); // COMMONGROUP_IMU + + RosQuaternionFromVnVector4(imu_msg.orientation, quaternion); + RosVector3FromVnVector3(imu_msg.angular_velocity, angular_rate); + RosVector3FromVnVector3(imu_msg.linear_acceleration, linear_accel); + } else { + // TODO + } } -void asciiOrBinaryAsyncMessageReceived(void* userData, vn::protocol::uart::Packet& p, size_t index){ - using namespace vn::protocol::uart; - ImuVn100* imu = (ImuVn100*) userData; - if (!p.isCompatible( - (COMMONGROUP_QUATERNION | COMMONGROUP_IMU | - COMMONGROUP_MAGPRES | COMMONGROUP_SYNCINCNT), - TIMEGROUP_NONE, - IMUGROUP_NONE, - GPSGROUP_NONE, - ATTITUDEGROUP_NONE, - INSGROUP_NONE)) - // Not the type of binary packet we are expecting. - return; - - if (!p.isValid()) { - ROS_WARN("Vn: Invalid packet received. CRC or checksum failed."); - return; - } - - imu->PublishData(p); +void asciiOrBinaryAsyncMessageReceived(void* userData, + vn::protocol::uart::Packet& p, + size_t index) { + using namespace vn::protocol::uart; + ImuVn100* imu = (ImuVn100*)userData; + if (!p.isCompatible((COMMONGROUP_QUATERNION | COMMONGROUP_IMU | + COMMONGROUP_MAGPRES | COMMONGROUP_SYNCINCNT), + TIMEGROUP_NONE, IMUGROUP_NONE, GPSGROUP_NONE, + ATTITUDEGROUP_NONE, INSGROUP_NONE)) + // Not the type of binary packet we are expecting. + return; + + if (!p.isValid()) { + ROS_WARN("Vn: Invalid packet received. CRC or checksum failed."); + return; + } + + imu->PublishData(p); } void errorMessageReceived(void* userData, vn::protocol::uart::Packet& p, size_t index) { - using vn::protocol::uart::SensorError; - ImuVn100* imu = (ImuVn100*) userData; - SensorError se = p.parseError(); + using vn::protocol::uart::SensorError; + ImuVn100* imu = (ImuVn100*)userData; + SensorError se = p.parseError(); - if (se == 0) return; + if (se == 0) return; - switch (se) { + switch (se) { case SensorError::ERR_HARD_FAULT: - ROS_ERROR("VN: Hard fault. Processor will force restart."); - break; - case SensorError::ERR_SERIAL_BUFFER_OVERFLOW: ///< Serial buffer overflow. - // We tried sending some kind of crazy long command which is impossible - // Throw because the developper shouldn't do this. - throw std::runtime_error("VN: Serial buffer overflow."); - break; - case SensorError::ERR_INVALID_CHECKSUM: ///< Invalid checksum. - ROS_WARN("VN: Invalid checksum on packet %s", std::to_string(index).c_str()); - break; - case SensorError::ERR_INVALID_COMMAND: ///< Invalid command. - ROS_WARN("VN: Invalid command on packet %s", std::to_string(index).c_str()); - break; - case SensorError::ERR_NOT_ENOUGH_PARAMETERS: ///< Not enough parameters. - ROS_WARN("VN: Not enough parameters."); - break; - case SensorError::ERR_TOO_MANY_PARAMETERS: ///< Too many parameters. - ROS_WARN("VN: Too many parameters."); - break; - case SensorError::ERR_INVALID_PARAMETER: ///< Invalid parameter. - ROS_WARN("VN: Invalid parameter."); - break; - case SensorError::ERR_INVALID_REGISTER: ///< Invalid register. - ROS_WARN("VN: Invalid register."); - break; - case SensorError::ERR_UNAUTHORIZED_ACCESS: ///< Unauthorized access. - ROS_WARN("VN: Unauthorized access to a register."); - break; - case SensorError::ERR_WATCHDOG_RESET: ///< Watchdog reset - ROS_WARN("VN: Watchdog reset has occured. VN should have restarted within 50 ms."); - break; - case SensorError::ERR_OUTPUT_BUFFER_OVERFLOW: ///< Output buffer overflow. - ROS_WARN("VN: Output buffer overflow."); - break; - case SensorError::ERR_INSUFFICIENT_BAUD_RATE: ///< Insufficient baud rate. - ROS_WARN("VN: Insufficient baud rate for requested async data output and rate."); - break; - case SensorError::ERR_ERROR_BUFFER_OVERFLOW: ///< Error buffer overflow. - ROS_WARN("VN: System error buffer overflow."); - break; + ROS_ERROR("VN: Hard fault. Processor will force restart."); + break; + case SensorError::ERR_SERIAL_BUFFER_OVERFLOW: ///< Serial buffer overflow. + // We tried sending some kind of crazy long command which is impossible + // Throw because the developper shouldn't do this. + throw std::runtime_error("VN: Serial buffer overflow."); + break; + case SensorError::ERR_INVALID_CHECKSUM: ///< Invalid checksum. + ROS_WARN("VN: Invalid checksum on packet %s", + std::to_string(index).c_str()); + break; + case SensorError::ERR_INVALID_COMMAND: ///< Invalid command. + ROS_WARN("VN: Invalid command on packet %s", + std::to_string(index).c_str()); + break; + case SensorError::ERR_NOT_ENOUGH_PARAMETERS: ///< Not enough parameters. + ROS_WARN("VN: Not enough parameters."); + break; + case SensorError::ERR_TOO_MANY_PARAMETERS: ///< Too many parameters. + ROS_WARN("VN: Too many parameters."); + break; + case SensorError::ERR_INVALID_PARAMETER: ///< Invalid parameter. + ROS_WARN("VN: Invalid parameter."); + break; + case SensorError::ERR_INVALID_REGISTER: ///< Invalid register. + ROS_WARN("VN: Invalid register."); + break; + case SensorError::ERR_UNAUTHORIZED_ACCESS: ///< Unauthorized access. + ROS_WARN("VN: Unauthorized access to a register."); + break; + case SensorError::ERR_WATCHDOG_RESET: ///< Watchdog reset + ROS_WARN( + "VN: Watchdog reset has occured. VN should have restarted within 50 " + "ms."); + break; + case SensorError::ERR_OUTPUT_BUFFER_OVERFLOW: ///< Output buffer overflow. + ROS_WARN("VN: Output buffer overflow."); + break; + case SensorError::ERR_INSUFFICIENT_BAUD_RATE: ///< Insufficient baud rate. + ROS_WARN( + "VN: Insufficient baud rate for requested async data output and " + "rate."); + break; + case SensorError::ERR_ERROR_BUFFER_OVERFLOW: ///< Error buffer overflow. + ROS_WARN("VN: System error buffer overflow."); + break; default: - throw std::runtime_error("VN: Unknown error code " + std::to_string(se)); - break; - } + throw std::runtime_error("VN: Unknown error code " + std::to_string(se)); + break; + } } -} // end namespace - +} // end namespace From 1f89e5e44b128950df064bd5d578c98f8e5fd319 Mon Sep 17 00:00:00 2001 From: Andre Nguyen Date: Sat, 27 Feb 2016 20:49:46 -0500 Subject: [PATCH 09/10] restructure code to accomodate ascii case --- include/imu_vn_100/imu_vn_100.h | 1 + src/imu_vn_100.cpp | 98 ++++++++++++++++++++------------- 2 files changed, 60 insertions(+), 39 deletions(-) diff --git a/include/imu_vn_100/imu_vn_100.h b/include/imu_vn_100/imu_vn_100.h index bda6cd1..9b9a6da 100644 --- a/include/imu_vn_100/imu_vn_100.h +++ b/include/imu_vn_100/imu_vn_100.h @@ -106,6 +106,7 @@ class ImuVn100 { const SyncInfo sync_info() const { return sync_info_; } + bool IsBinaryOutput() { return binary_output_; } private: ros::NodeHandle pnh_; vn::sensors::VnSensor imu_; diff --git a/src/imu_vn_100.cpp b/src/imu_vn_100.cpp index a18808e..711bfd9 100644 --- a/src/imu_vn_100.cpp +++ b/src/imu_vn_100.cpp @@ -40,15 +40,6 @@ void RosVector3FromVnVector3(geometry_msgs::Vector3& ros_vec3, void RosQuaternionFromVnVector4(geometry_msgs::Quaternion& ros_quat, const vn::math::vec4f& vn_vec4); -/** - * @brief FillImuMessage - * @param imu_msg - * @param p - * @param binary_output - */ -void FillImuMessage(sensor_msgs::Imu& imu_msg, vn::protocol::uart::Packet& p, - bool binary_output); - /** * @brief asciiOrBinaryAsyncMessageReceived * @param userData @@ -112,7 +103,6 @@ ImuVn100::ImuVn100(const ros::NodeHandle& pnh) baudrate_(921600), frame_id_(std::string("imu")) { Initialize(); - // TODO pointer hack } ImuVn100::~ImuVn100() { Disconnect(); } @@ -145,6 +135,12 @@ void ImuVn100::LoadParameters() { pnh_.param("sync_pulse_width_us", sync_info_.pulse_width_us, 1000); pnh_.param("binary_output", binary_output_, true); + + if (!binary_output_ && (enable_pres_ | enable_temp_)) { + ROS_ERROR("VN: Ascii mode cannot support pressure and temp."); + enable_pres_ = enable_temp_ = false; + } + int vn_serial_output_tmp = 4; // init on invalid number pnh_.param("vn_serial_output", vn_serial_output_tmp, 1); switch (vn_serial_output_tmp) { @@ -277,7 +273,14 @@ void ImuVn100::Stream(bool async) { imu_.writeBinaryOutput1(bor, true); } else { // Set the ASCII output data type and data rate - imu_.writeAsyncDataOutputType(VNIMU, true); + vn::sensors::BinaryOutputRegister bor( + vn_serial_output_, 0, COMMONGROUP_NONE, TIMEGROUP_NONE, IMUGROUP_NONE, + GPSGROUP_NONE, ATTITUDEGROUP_NONE, INSGROUP_NONE); + // disable all the binary outputs + imu_.writeBinaryOutput1(bor, true); + imu_.writeBinaryOutput2(bor, true); + imu_.writeBinaryOutput3(bor, true); + imu_.writeAsyncDataOutputType(VNQMR, true); } // add a callback function for new data event @@ -310,13 +313,34 @@ void ImuVn100::PublishData(vn::protocol::uart::Packet& p) { imu_msg.header.stamp = ros::Time::now(); imu_msg.header.frame_id = frame_id_; - FillImuMessage(imu_msg, p, binary_output_); + vn::math::vec4f quaternion; + vn::math::vec3f linear_accel; + vn::math::vec3f angular_rate; + vn::math::vec3f magnetometer; + if (binary_output_) { + // Note: With this library, we are responsible for extracting the data + // in the appropriate order! Need to refer to manual and to how we + // configured the common output group + quaternion = p.extractVec4f(); // COMMONGROUP_QUATERNION + // NOTE: The IMU angular velocity and linear acceleration outputs are + // swapped. And also why are they different? + linear_accel = p.extractVec3f(); // COMMONGROUP_IMU + angular_rate = p.extractVec3f(); // COMMONGROUP_IMU + magnetometer = p.extractVec3f(); // COMMONGROUP_MAGPRES + } else { + // In ascii mode, linear acceleration and angular velocity are NOT swapped + p.parseVNQMR(&quaternion, &magnetometer, &linear_accel, &angular_rate); + } + + RosQuaternionFromVnVector4(imu_msg.orientation, quaternion); + RosVector3FromVnVector3(imu_msg.angular_velocity, angular_rate); + RosVector3FromVnVector3(imu_msg.linear_acceleration, linear_accel); + pd_imu_.Publish(imu_msg); if (enable_mag_) { sensor_msgs::MagneticField mag_msg; mag_msg.header = imu_msg.header; - vn::math::vec3f magnetometer = p.extractVec3f(); // COMMONGROUP_MAGPRES RosVector3FromVnVector3(mag_msg.magnetic_field, magnetometer); pd_mag_.Publish(mag_msg); } @@ -358,37 +382,33 @@ void RosQuaternionFromVnVector4(geometry_msgs::Quaternion& ros_quat, ros_quat.w = vn_vec4[3]; } -void FillImuMessage(sensor_msgs::Imu& imu_msg, vn::protocol::uart::Packet& p, - bool binary_output) { - if (binary_output) { - // Note: With this library, we are responsible for extracting the data - // in the appropriate order! Need to refer to manual and to how we - // configured the common output group - vn::math::vec4f quaternion = p.extractVec4f(); // COMMONGROUP_QUATERNION - // NOTE: The IMU angular velocity and linear acceleration outputs are - // swapped. And also why are they different? - vn::math::vec3f linear_accel = p.extractVec3f(); // COMMONGROUP_IMU - vn::math::vec3f angular_rate = p.extractVec3f(); // COMMONGROUP_IMU - - RosQuaternionFromVnVector4(imu_msg.orientation, quaternion); - RosVector3FromVnVector3(imu_msg.angular_velocity, angular_rate); - RosVector3FromVnVector3(imu_msg.linear_acceleration, linear_accel); - } else { - // TODO - } -} - void asciiOrBinaryAsyncMessageReceived(void* userData, vn::protocol::uart::Packet& p, size_t index) { using namespace vn::protocol::uart; ImuVn100* imu = (ImuVn100*)userData; - if (!p.isCompatible((COMMONGROUP_QUATERNION | COMMONGROUP_IMU | - COMMONGROUP_MAGPRES | COMMONGROUP_SYNCINCNT), - TIMEGROUP_NONE, IMUGROUP_NONE, GPSGROUP_NONE, - ATTITUDEGROUP_NONE, INSGROUP_NONE)) - // Not the type of binary packet we are expecting. - return; + + if (imu->IsBinaryOutput()) { + if (!p.isCompatible((COMMONGROUP_QUATERNION | COMMONGROUP_IMU | + COMMONGROUP_MAGPRES | COMMONGROUP_SYNCINCNT), + TIMEGROUP_NONE, IMUGROUP_NONE, GPSGROUP_NONE, + ATTITUDEGROUP_NONE, INSGROUP_NONE)) { + // Not the type of binary packet we are expecting. + ROS_WARN("VN: Received malformatted binary packet."); + return; + } + } else { + // ascii format + if (p.type() != vn::protocol::uart::Packet::TYPE_ASCII) { + ROS_WARN("VN: Requested ascii, but got wrong type."); + return; + } + + if (p.determineAsciiAsyncType() != vn::protocol::uart::VNQMR) { + ROS_WARN("VN: Wrong ascii format received."); + return; + } + } if (!p.isValid()) { ROS_WARN("Vn: Invalid packet received. CRC or checksum failed."); From ac23b579be5f701354315f0b0eb4f7e6264bf41d Mon Sep 17 00:00:00 2001 From: Andre Phu-Van Nguyen Date: Mon, 29 Feb 2016 01:07:44 -0500 Subject: [PATCH 10/10] change launch file back to output 2 change verbosity level of some messages catkinize vnproglib to be included in ros --- CMakeLists.txt | 2 +- ext/vnproglib-1.1.0.115/CMakeLists.txt | 11 +++++++++++ ext/vnproglib-1.1.0.115/package.xml | 11 +++++++++++ launch/vn_100_cont.launch | 4 ++-- src/imu_vn_100.cpp | 8 ++++---- 5 files changed, 29 insertions(+), 7 deletions(-) create mode 100644 ext/vnproglib-1.1.0.115/package.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index 73cd37f..cd5ef77 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(imu_vn_100) -# build the vnproflib first +# build the vnproglib first add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/ext/vnproglib-1.1.0.115) add_definitions("-std=c++11") diff --git a/ext/vnproglib-1.1.0.115/CMakeLists.txt b/ext/vnproglib-1.1.0.115/CMakeLists.txt index 523c0c2..9247d39 100755 --- a/ext/vnproglib-1.1.0.115/CMakeLists.txt +++ b/ext/vnproglib-1.1.0.115/CMakeLists.txt @@ -1,5 +1,7 @@ cmake_minimum_required(VERSION 2.8.4) +project(vnproglib) + set(CMAKE_SUPPRESS_REGENERATION TRUE) add_definitions(-DPL150) #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") @@ -14,6 +16,15 @@ option(PYTHON "Build for Python library." OFF) option(BUILD_GRAPHICS "Build in the graphics library." OFF) #option(INTERNAL "Build internal portions of the library." OFF) +find_package(catkin REQUIRED) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS + DEPENDS +) + set_property(GLOBAL PROPERTY USE_FOLDERS ON) set(top_sources diff --git a/ext/vnproglib-1.1.0.115/package.xml b/ext/vnproglib-1.1.0.115/package.xml new file mode 100644 index 0000000..94db3e6 --- /dev/null +++ b/ext/vnproglib-1.1.0.115/package.xml @@ -0,0 +1,11 @@ + + + vnproglib + 1.0.115 + VectorNav beta programming library + No body + See VectorNav release + catkin + + + diff --git a/launch/vn_100_cont.launch b/launch/vn_100_cont.launch index 26f1078..6973e1b 100644 --- a/launch/vn_100_cont.launch +++ b/launch/vn_100_cont.launch @@ -9,7 +9,7 @@ - + @@ -25,7 +25,7 @@ - + diff --git a/src/imu_vn_100.cpp b/src/imu_vn_100.cpp index 711bfd9..6eded02 100644 --- a/src/imu_vn_100.cpp +++ b/src/imu_vn_100.cpp @@ -198,16 +198,16 @@ void ImuVn100::Initialize() { ROS_INFO("Set serial baudrate to %d", baudrate_); imu_.writeSerialBaudRate(baudrate_, true); - ROS_DEBUG("Disconnecting the device"); + ROS_INFO("Disconnecting the device"); imu_.disconnect(); ros::Duration(0.5).sleep(); } catch (std::exception except) { - ROS_DEBUG("Failed to open device with default baudrate with exception: %s", + ROS_INFO("Failed to open device with default baudrate with exception: %s", except.what()); } // Open with the desired baud rate - ROS_DEBUG("Reconnecting to device"); + ROS_INFO("Reconnecting to device"); imu_.connect(port_, baudrate_); ros::Duration(0.5).sleep(); ROS_INFO("Connected to device at %s", port_.c_str()); @@ -306,7 +306,7 @@ void ImuVn100::Resume(bool need_reply) {} void ImuVn100::Idle(bool need_reply) {} -void ImuVn100::Disconnect() {} +void ImuVn100::Disconnect() { imu_.disconnect(); } void ImuVn100::PublishData(vn::protocol::uart::Packet& p) { sensor_msgs::Imu imu_msg;