From 8282be62b896e55b4f3aeed59e18a0c65cca5d74 Mon Sep 17 00:00:00 2001 From: "r.koeppe" Date: Tue, 18 Jun 2024 11:27:32 +0000 Subject: [PATCH] added libs --- .gitmodules | 7 + CMakeLists.txt | 12 +- Libs/can-transceiver-lib | 1 + Libs/cantools | 1 + Libs/vn-interface/CMakeLists.txt | 69 ++ Libs/vn-interface/Config.cmake.in | 4 + Libs/vn-interface/gcc-arm-none-eabi.cmake | 23 + Libs/vn-interface/include/vn-interface/enum.h | 670 ++++++++++++++ .../include/vn-interface/frames.h | 50 ++ .../include/vn-interface/helper.h | 57 ++ Libs/vn-interface/include/vn-interface/int.h | 29 + .../include/vn-interface/math/matrix.h | 69 ++ .../include/vn-interface/math/vector.h | 148 +++ .../include/vn-interface/registers.h | 845 ++++++++++++++++++ .../vn-interface/include/vn-interface/types.h | 56 ++ Libs/vn-interface/src/helper.cpp | 3 + Libs/vn-interface/src/math/matrix.c | 20 + Libs/vn-interface/src/math/vector.c | 118 +++ Libs/vn-interface/src/registers.c | 15 + 19 files changed, 2193 insertions(+), 4 deletions(-) create mode 100644 .gitmodules create mode 160000 Libs/can-transceiver-lib create mode 160000 Libs/cantools create mode 100644 Libs/vn-interface/CMakeLists.txt create mode 100644 Libs/vn-interface/Config.cmake.in create mode 100644 Libs/vn-interface/gcc-arm-none-eabi.cmake create mode 100644 Libs/vn-interface/include/vn-interface/enum.h create mode 100644 Libs/vn-interface/include/vn-interface/frames.h create mode 100644 Libs/vn-interface/include/vn-interface/helper.h create mode 100644 Libs/vn-interface/include/vn-interface/int.h create mode 100644 Libs/vn-interface/include/vn-interface/math/matrix.h create mode 100644 Libs/vn-interface/include/vn-interface/math/vector.h create mode 100644 Libs/vn-interface/include/vn-interface/registers.h create mode 100644 Libs/vn-interface/include/vn-interface/types.h create mode 100644 Libs/vn-interface/src/helper.cpp create mode 100644 Libs/vn-interface/src/math/matrix.c create mode 100644 Libs/vn-interface/src/math/vector.c create mode 100644 Libs/vn-interface/src/registers.c diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..b91341b --- /dev/null +++ b/.gitmodules @@ -0,0 +1,7 @@ +[submodule "Libs/cantools"] + path = Libs/cantools + url = https://github.com/rimakoe/cantools.git + branch = extend_codegen +[submodule "Libs/can-transceiver-lib"] + path = Libs/can-transceiver-lib + url = https://github.com/rimakoe/can-transceiver-lib.git diff --git a/CMakeLists.txt b/CMakeLists.txt index 85e024f..8d0a7cb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,8 +41,8 @@ add_executable(${CMAKE_PROJECT_NAME}) # Add STM32CubeMX generated sources add_subdirectory(cmake/stm32cubemx) -find_package(vn-interface) -find_package(can-transceiver-lib) +#find_package(vn-interface) +#find_package(can-transceiver-lib) # Link directories setup target_link_directories(${CMAKE_PROJECT_NAME} PRIVATE @@ -58,6 +58,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE # Add include paths target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE Converter/Inc + Libs/can-transceiver-lib/include + Libs/vn-interface/include ) # Add project symbols (macros) @@ -66,9 +68,11 @@ target_compile_definitions(${CMAKE_PROJECT_NAME} PRIVATE ) # Add linked libraries +# We want to link them very explicitly from our build folder in this repo for the case of multiple repos and different MCUs. +# Linking the usual way in /usr/local could cause difficulties in designing such libraries due to different compiler flags for different MCU types. target_link_libraries(${CMAKE_PROJECT_NAME} stm32cubemx - vn-interface::vn-interface - canlib::can-transceiver-lib # Add user defined libraries + ${CMAKE_SOURCE_DIR}/Libs/can-transceiver-lib/build/libcan-transceiver-lib.a + ${CMAKE_SOURCE_DIR}/Libs/vn-interface/build/libvn-interface.a ) diff --git a/Libs/can-transceiver-lib b/Libs/can-transceiver-lib new file mode 160000 index 0000000..05e7847 --- /dev/null +++ b/Libs/can-transceiver-lib @@ -0,0 +1 @@ +Subproject commit 05e78474582768dc685d5b6ece94e34f3853c5d8 diff --git a/Libs/cantools b/Libs/cantools new file mode 160000 index 0000000..f26224f --- /dev/null +++ b/Libs/cantools @@ -0,0 +1 @@ +Subproject commit f26224f423408ca663956379637de2f00ceb9013 diff --git a/Libs/vn-interface/CMakeLists.txt b/Libs/vn-interface/CMakeLists.txt new file mode 100644 index 0000000..54f727a --- /dev/null +++ b/Libs/vn-interface/CMakeLists.txt @@ -0,0 +1,69 @@ +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/gcc-arm-none-eabi.cmake) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(BUILD_TYPE Debug) + +project(vn-interface) + +include_directories(${CMAKE_SOURCE_DIR}/include) + +# include for standard directories +include(GNUInstallDirs) + +# include for creating config.cmake +include(CMakePackageConfigHelpers) + +set(SOURCES src/helper.cpp) + +set(CPU_PARAMETERS + -mcpu=cortex-m4 + -mthumb + -mfpu=fpv4-sp-d16 + -mfloat-abi=hard) + +add_library(${PROJECT_NAME} STATIC ${SOURCES}) + +target_compile_options(${PROJECT_NAME} PRIVATE + ${CPU_PARAMETERS} + -Wall + -Wextra + -Wpedantic + -Wno-unused-parameter + $<$: + -Wno-volatile + -Wsuggest-override + -Wno-missing-field-initializers> + $<$:-Og -g3 -ggdb> + $<$:-Og -g0>) + +# Link the desire include directories of the library - different for build and install +target_include_directories(${PROJECT_NAME} + PUBLIC + "$" + "$" + ) + +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}Targets + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +install(DIRECTORY include/${PROJECT_NAME} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) + +install(EXPORT ${PROJECT_NAME}Targets + FILE ${PROJECT_NAME}Targets.cmake + NAMESPACE vn-interface:: + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME} +) + +configure_package_config_file(${CMAKE_CURRENT_SOURCE_DIR}/Config.cmake.in + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME} +) + +install(FILES + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME} +) diff --git a/Libs/vn-interface/Config.cmake.in b/Libs/vn-interface/Config.cmake.in new file mode 100644 index 0000000..50d94d4 --- /dev/null +++ b/Libs/vn-interface/Config.cmake.in @@ -0,0 +1,4 @@ +@PACKAGE_INIT@ + +include("${CMAKE_CURRENT_LIST_DIR}/vn-interfaceTargets.cmake") + diff --git a/Libs/vn-interface/gcc-arm-none-eabi.cmake b/Libs/vn-interface/gcc-arm-none-eabi.cmake new file mode 100644 index 0000000..0cb7d34 --- /dev/null +++ b/Libs/vn-interface/gcc-arm-none-eabi.cmake @@ -0,0 +1,23 @@ +set(CMAKE_SYSTEM_NAME Generic) +set(CMAKE_SYSTEM_PROCESSOR arm) + +# Some default GCC settings +set(TOOLCHAIN_PREFIX arm-none-eabi-) +set(FLAGS + "-fdata-sections -ffunction-sections \ + --specs=nano.specs -Wl,--gc-sections") +set(CPP_FLAGS + "-fno-rtti -fno-exceptions \ + -fno-threadsafe-statics") + +set(CMAKE_C_COMPILER ${TOOLCHAIN_PREFIX}gcc ${FLAGS}) +set(CMAKE_ASM_COMPILER ${CMAKE_C_COMPILER}) +set(CMAKE_CXX_COMPILER ${TOOLCHAIN_PREFIX}g++ ${FLAGS} ${CPP_FLAGS}) +set(CMAKE_OBJCOPY ${TOOLCHAIN_PREFIX}objcopy) +set(CMAKE_SIZE ${TOOLCHAIN_PREFIX}size) + +set(CMAKE_EXECUTABLE_SUFFIX_ASM ".elf") +set(CMAKE_EXECUTABLE_SUFFIX_C ".elf") +set(CMAKE_EXECUTABLE_SUFFIX_CXX ".elf") +set(CMAKE_LINK_LIBRARY_SUFFIX ".elf") +set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY) \ No newline at end of file diff --git a/Libs/vn-interface/include/vn-interface/enum.h b/Libs/vn-interface/include/vn-interface/enum.h new file mode 100644 index 0000000..617d88c --- /dev/null +++ b/Libs/vn-interface/include/vn-interface/enum.h @@ -0,0 +1,670 @@ +#ifndef VNENUM_H_INCLUDED +#define VNENUM_H_INCLUDED + +/** \brief The different types of UART packets. */ +typedef enum +{ + PACKETTYPE_BINARY, /**< Binary packet. */ + PACKETTYPE_ASCII, /**< ASCII packet. */ + PACKETTYPE_UNKNOWN /**< Unknown packet type. */ +} PacketType; + +/** \brief The available binary output groups. */ +typedef enum +{ + BINARYGROUPTYPE_COMMON = 0x01, /**< Common group. */ + BINARYGROUPTYPE_TIME = 0x02, /**< Time group. */ + BINARYGROUPTYPE_IMU = 0x04, /**< IMU group. */ + BINARYGROUPTYPE_GPS = 0x08, /**< GPS group. */ + BINARYGROUPTYPE_ATTITUDE = 0x10, /**< Attitude group. */ + BINARYGROUPTYPE_INS = 0x20, /**< INS group. */ + BINARYGROUPTYPE_GPS2 = 0x40 /**< GPS2 group. */ +} BinaryGroupType; + +/** \brief Async modes for the Binary Output registers. */ +typedef enum +{ + ASYNCMODE_NONE = 0, /**< None. */ + ASYNCMODE_PORT1 = 1, /**< Serial port 1. */ + ASYNCMODE_PORT2 = 2, /**< Serial port 2. */ + ASYNCMODE_BOTH = 3 /**< Both serial ports. */ +} AsyncMode; + +/** \brief Flags for the binary group 1 'Common' in the binary output registers. */ +typedef enum +{ + 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. */ +} CommonGroup; + +/** \brief Flags for the binary group 2 'Time' in the binary output registers. */ +typedef enum +{ + TIMEGROUP_NONE = 0x0000, /**< None. */ + TIMEGROUP_TIMESTARTUP = 0x0001, /**< TimeStartup. */ + TIMEGROUP_TIMEGPS = 0x0002, /**< TimeGps. */ + TIMEGROUP_GPSTOW = 0x0004, /**< GpsTow. */ + TIMEGROUP_GPSWEEK = 0x0008, /**< GpsWeek. */ + TIMEGROUP_TIMESYNCIN = 0x0010, /**< TimeSyncIn. */ + TIMEGROUP_TIMEGPSPPS = 0x0020, /**< TimeGpsPps. */ + TIMEGROUP_TIMEUTC = 0x0040, /**< TimeUTC. */ + TIMEGROUP_SYNCINCNT = 0x0080, /**< SyncInCnt. */ + TIMEGROUP_SYNCOUTCNT = 0x0100, /**< SyncOutCnt. */ + TIMEGROUP_TIMESTATUS = 0x0200 /**< TimeStatus. */ +} TimeGroup; + +/** \brief Flags for the binary group 3 'IMU' in the binary output registers. */ +typedef enum +{ + 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. */ + #ifdef EXTRA + , + IMUGROUP_RAW = 0x1000 /**< Raw. */ + #endif +} ImuGroup; + +/** \brief Flags for the binary group 4 'GPS' in the binary output registers. */ +typedef enum +{ + GPSGROUP_NONE = 0x0000, /**< None. */ + GPSGROUP_UTC = 0x0001, /**< UTC. */ + GPSGROUP_TOW = 0x0002, /**< Tow. */ + GPSGROUP_WEEK = 0x0004, /**< Week. */ + GPSGROUP_NUMSATS = 0x0008, /**< NumSats. */ + GPSGROUP_FIX = 0x0010, /**< Fix. */ + GPSGROUP_POSLLA = 0x0020, /**< PosLla. */ + GPSGROUP_POSECEF = 0x0040, /**< PosEcef. */ + GPSGROUP_VELNED = 0x0080, /**< VelNed. */ + GPSGROUP_VELECEF = 0x0100, /**< VelEcef. */ + GPSGROUP_POSU = 0x0200, /**< PosU. */ + GPSGROUP_VELU = 0x0400, /**< VelU. */ + GPSGROUP_TIMEU = 0x0800, /**< TimeU. */ + GPSGROUP_TIMEINFO = 0x1000, /**< TimeInfo. */ + GPSGROUP_DOP = 0x2000 /**< DOP. */ + + #ifdef EXTRA + , + GPSGROUP_SVSTAT = 0x1000 /**< SvStat. */ + #endif +} GpsGroup; + +/** \brief Flags for the binary group 5 'Attitude' in the binary output registers. */ +typedef enum +{ + 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. */ + #ifdef EXTRA + , + ATTITUDEGROUP_YPRRATE = 0x0200, /**< YprRate. */ + ATTITUDEGROUP_STATEAHRS = 0x0400, /**< StateAhrs. */ + ATTITUDEGROUP_COVAHRS = 0x0800 /**< CovAhrs. */ + #endif +} AttitudeGroup; + +/** \brief Flags for the binary group 6 'INS' in the binary output registers. */ +typedef enum +{ + 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. */ + #ifdef EXTRA + , + INSGROUP_STATEINS = 0x0800, /**< StateIns. */ + INSGROUP_COVINS = 0x1000 /**< CovIns. */ + #endif +} InsGroup; + +/** \brief Enumeration of the velocity types */ +typedef enum +{ + CDVEL_None, + CDVEL_GpsNed, + CDVEL_GpsEcef, + CDVEL_EstimatedNed, + CDVEL_EstimatedEcef, + CDVEL_EstimatedBody +} VelocityType; + +/** \brief Errors that the VectorNav sensor can report. */ +typedef enum +{ + 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. */ +} SensorError; + +/** \brief Enumeration of the various error messages used by the library. */ +typedef enum +{ + /** Indicates there were no errors encountered. */ + E_NONE, + + /** Indicates an unknown error occurred. */ + E_UNKNOWN, + + /** Indicates a provided buffer was too small to complete the action. */ + E_BUFFER_TOO_SMALL, + + /** Indicates a provided value is not valid. */ + E_INVALID_VALUE, + + /** Indicates the requested functionality is currently not implemented. */ + E_NOT_IMPLEMENTED, + + /** Indicates the requested functionality is not supported. */ + E_NOT_SUPPORTED, + + /** Indicates the requested item was not found. */ + E_NOT_FOUND, + + /** Indicates the operation timed out. */ + E_TIMEOUT, + + /** Indicates insufficient permission to perform the operation. */ + E_PERMISSION_DENIED, + + /** Indicates an invalid operation was attempted. */ + E_INVALID_OPERATION, + + /** Indicates an event was signaled. */ + E_SIGNALED, + + /** Indicates either not enough memory is available or no memory was allocated */ + E_MEMORY_NOT_ALLOCATED, + + /** VectorNav sensor hard fault (Code 1). */ + E_SENSOR_HARD_FAULT = 1001, + + /** VectorNav sensor serial buffer overflow (Code 2). */ + E_SENSOR_SERIAL_BUFFER_OVERFLOW = 1002, + + /** VectorNav sensor invalid checksum (Code 3). */ + E_SENSOR_INVALID_CHECKSUM = 1003, + + /** VectorNav sensor invalid command (Code 4). */ + E_SENSOR_INVALID_COMMAND = 1004, + + /** VectorNav sensor not enough parameters (Code 5). */ + E_SENSOR_NOT_ENOUGH_PARAMETERS = 1005, + + /** VectorNav sensor too many parameters (Code 6). */ + E_SENSOR_TOO_MANY_PARAMETERS = 1006, + + /** VectorNav sensor invalid parameter (Code 7). */ + E_SENSOR_INVALID_PARAMETER = 1007, + + /** VectorNav sensor invalid register (Code 8). */ + E_SENSOR_INVALID_REGISTER = 1008, + + /** VectorNav sensor unauthorized access (Code 9). */ + E_SENSOR_UNAUTHORIZED_ACCESS = 1009, + + /** VectorNav sensor watchdog reset (Code 10). */ + E_SENSOR_WATCHDOG_RESET = 1010, + + /** VectorNav sensor output buffer overflow (Code 11). */ + E_SENSOR_OUTPUT_BUFFER_OVERFLOW = 1011, + + /** VectorNav sensor insufficient baud rate (Code 12). */ + E_SENSOR_INSUFFICIENT_BAUD_RATE = 1012, + + /** VectorNav sensor error buffer overflow (Code 13). */ + E_SENSOR_ERROR_BUFFER_OVERFLOW = 1013, + + E_DATA_NOT_ELLIPTICAL = 2001, /**< \brief Data set not elliptical. */ + E_ILL_CONDITIONED = 2002, /**< \brief Algorithm had a bad condition. */ + E_EXCEEDED_MAX_ITERATIONS = 2003, /**< \brief Algorithm exceeded the number of interations it is allowed. */ + E_BAD_FINAL_INTERATION = 2004, /**< \brief Algorithm's last interation changed exceeded threshold. */ + E_INSUFFICIENT_DATA = 2005, /**< \brief Not enough data points were provided. */ + + /** \brief Errors that the VectorNav bootload can report. */ + E_BOOTLOADER_NONE = 3000, /**< No Error, send next record*/ + E_BOOTLOADER_INVALID_COMMAND = 3001, /**< Problem with VNX record, abort */ + E_BOOTLOADER_INVALID_RECORD_TYPE = 3002, /**< Problem with VNX record, abort */ + E_BOOTLOADER_INVALID_BYTE_COUNT = 3003, /**< Problem with VNX record, abort */ + E_BOOTLOADER_INVALID_MEMORY_ADDRESS = 3004, /**< Problem with VNX record, abort */ + E_BOOTLOADER_COMM_ERROR = 3005, /**< Checksum error, resend record */ + E_BOOTLOADER_INVALID_HEX_FILE = 3006, /**< Problem with VNX record, abort */ + E_BOOTLOADER_DECRYPTION_ERROR = 3007, /**< Invalid VNX file or record sent out of order, abort */ + E_BOOTLOADER_INVALID_BLOCK_CRC = 3008, /**< Data verification failed, abort */ + E_BOOTLOADER_INVALID_PROGRAM_CRC = 3009, /**< Problemw ith firmware on device */ + E_BOOTLOADER_INVALID_PROGRAM_SIZE = 3010, /**< Problemw ith firmware on device */ + E_BOOTLOADER_MAX_RETRY_COUNT = 3011, /**< Too many errors, abort */ + E_BOOTLOADER_TIMEOUT = 3012, /**< Timeout expired, reset */ + E_BOOTLOADER_RESERVED = 3013 /**< Contact VectorNav, abort */ + +} VnError; + +/** \brief Enumeration of the various error-detection algorithms used by the + * library. */ +typedef enum +{ + /** Signifies no error-detection should be performed. */ + VNERRORDETECTIONMODE_NONE, + + /** Signifies to use 8-bit XOR checksum. */ + VNERRORDETECTIONMODE_CHECKSUM, + + /** Signifies to use CRC16-CCITT algorithm. */ + VNERRORDETECTIONMODE_CRC + +} VnErrorDetectionMode; + +/** \brief Different modes for the SyncInMode field of the Synchronization Control register. */ +typedef enum +{ + #ifdef EXTRA + /** \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. + */ + VNSYNCINMODE_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. + */ + VNSYNCINMODE_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. + */ + VNSYNCINMODE_ASYNC2 = 2, + #endif + /** \brief Count number of trigger events on SYNC_IN pin. + */ + VNSYNCINMODE_COUNT = 3, + /** \brief Start IMU sampling on trigger of SYNC_IN pin. + */ + VNSYNCINMODE_IMU = 4, + /** \brief Output asynchronous message on trigger of SYNC_IN pin. + */ + VNSYNCINMODE_ASYNC = 5 +} VnSyncInMode; + +/** \brief Different modes for the SyncInEdge field of the Synchronization Control register. */ +typedef enum +{ + /** \brief Trigger on the rising edge on the SYNC_IN pin. + */ + VNSYNCINEDGE_RISING = 0, + /** \brief Trigger on the falling edge on the SYNC_IN pin. + */ + VNSYNCINEDGE_FALLING = 1 +} VnSyncInEdge; + +/** \brief Different modes for the SyncOutMode field of the Synchronization Control register. */ +typedef enum +{ + /** \brief None. + */ + VNSYNCOUTMODE_NONE = 0, + /** \brief Trigger at start of IMU sampling. + */ + VNSYNCOUTMODE_ITEMSTART = 1, + /** \brief Trigger when IMU measurements are available. + */ + VNSYNCOUTMODE_IMUREADY = 2, + /** \brief Trigger when attitude measurements are available. + */ + VNSYNCOUTMODE_INS = 3, + /** \brief Trigger on GPS PPS event when a 3D fix is valid. + */ + VNSYNCOUTMODE_GPSPPS = 6 +} VnSyncOutMode; + +/** \brief Different modes for the SyncOutPolarity field of the Synchronization Control register. */ +typedef enum +{ + /** \brief Negative pulse. + */ + VNSYNCOUTPOLARITY_NEGATIVE = 0, + /** \brief Positive pulse. + */ + VNSYNCOUTPOLARITY_POSITIVE = 1 +} VnSyncOutPolarity; + +/** \brief Counting modes for the Communication Protocol Control register. */ +typedef enum +{ + /** \brief Off. + */ + VNCOUNTMODE_NONE = 0, + /** \brief SyncIn counter. + */ + VNCOUNTMODE_SYNCINCOUNT = 1, + /** \brief SyncIn time. + */ + VNCOUNTMODE_SYNCINTIME = 2, + /** \brief SyncOut counter. + */ + VNCOUNTMODE_SYNCOUTCOUNTER = 3, + /** \brief GPS PPS time. + */ + VNCOUNTMODE_GPSPPS = 4 +} VnCountMode; + +/** \brief Status modes for the Communication Protocol Control register. */ +typedef enum +{ + /** \brief Off. + */ + VNSTATUSMODE_OFF = 0, + /** \brief VPE status. + */ + VNSTATUSMODE_VPESTATUS = 1, + /** \brief INS status. + */ + VNSTATUSMODE_INSSTATUS = 2 +} VnStatusMode; + +/** \brief Checksum modes for the Communication Protocol Control register. */ +typedef enum +{ + /** \brief Off. + */ + VNCHECKSUMMODE_OFF = 0, + /** \brief 8-bit checksum. + */ + VNCHECKSUMMODE_CHECKSUM = 1, + /** \brief 16-bit CRC. + */ + VNCHECKSUMMODE_CRC = 2 +} VnChecksumMode; + +/** \brief Error modes for the Communication Protocol Control register. */ +typedef enum +{ + /** \brief Ignore error. + */ + VNERRORMODE_IGNORE = 0, + /** \brief Send error. + */ + VNERRORMODE_SEND = 1, + /** \brief Send error and set ADOR register to off. + */ + VNERRORMODE_SENDANDOFF = 2 +} VnErrorMode; + +/** \brief Filter modes for the IMU Filtering Configuration register. */ +typedef enum +{ + /** \brief No filtering. + */ + VNFILTERMODE_NOFILTERING = 0, + /** \brief Filtering performed only on raw uncompensated IMU measurements. + */ + VNFILTERMODE_ONLYRAW = 1, + /** \brief Filtering performed only on compensated IMU measurements. + */ + VNFILTERMODE_ONLYCOMPENSATED = 2, + /** \brief Filtering performed on both uncompensated and compensated IMU measurements. + */ + VNFILTERMODE_BOTH = 3 +} VnFilterMode; + +/** \brief Integration frames for the Delta Theta and Delta Velocity Configuration register. */ +typedef enum +{ + /** \brief Body frame. + */ + VNINTEGRATIONFRAME_BODY = 0, + /** \brief NED frame. + */ + VNINTEGRATIONFRAME_NED = 1 +} VnIntegrationFrame; + +/** \brief Compensation modes for the Delta Theta and Delta Velocity configuration register. */ +typedef enum +{ + /** \brief None. + */ + VNCOMPENSATIONMODE_NONE = 0, + /** \brief Bias. + */ + VNCOMPENSATIONMODE_BIAS = 1 +} VnCompensationMode; + +/** \brief GPS fix modes for the GPS Solution - LLA register. */ +typedef enum +{ + /** \brief No fix. + */ + VNGPSFIX_NOFIX = 0, + /** \brief Time only. + */ + VNGPSFIX_TIMEONLY = 1, + /** \brief 2D. + */ + VNGPSFIX_2D = 2, + /** \brief 3D. + */ + VNGPSFIX_3D = 3 +} VnGpsFix; + +/** \brief GPS modes for the GPS Configuration register. */ +typedef enum +{ + /** \brief Use onboard GPS. + */ + VNGPSMODE_ONBOARDGPS = 0, + /** \brief Use external GPS. + */ + VNGPSMODE_EXTERNALGPS = 1, + /** \brief Use external VN-200 as GPS. + */ + VNGPSMODE_EXTERNALVN200GPS = 2 +} VnGpsMode; + +/** \brief GPS PPS mode for the GPS Configuration register. */ +typedef enum +{ + /** \brief GPS PPS signal on GPS_PPS pin and triggered on rising edge. + */ + VNPPSSOURCE_GPSPPSRISING = 0, + /** \brief GPS PPS signal on GPS_PPS pin and triggered on falling edge. + */ + VNPPSSOURCE_GPSPPSFALLING = 1, + /** \brief GPS PPS signal on SyncIn pin and triggered on rising edge. + */ + VNPPSSOURCE_SYNCINRISING = 2, + /** \brief GPS PPS signal on SyncIn pin and triggered on falling edge. + */ + VNPPSSOURCE_SYNCINFALLING = 3 +} VnPpsSource; + +/** \brief VPE Enable mode for the VPE Basic Control register. */ +typedef enum +{ + /** \brief Disable + */ + VNVPEENABLE_DISABLE = 0, + /** \brief Enable + */ + VNVPEENABLE_ENABLE = 1 +} VnVpeEnable; + +/** \brief VPE Heading modes used by the VPE Basic Control register. */ +typedef enum +{ + /** \brief Absolute heading. + */ + VNHEADINGMODE_ABSOLUTE = 0, + /** \brief Relative heading. + */ + VNHEADINGMODE_RELATIVE = 1, + /** \brief Indoor heading. + */ + VNHEADINGMODE_INDOOR = 2 +} VnHeadingMode; + +/** \brief VPE modes for the VPE Basic Control register. */ +typedef enum +{ + /** \brief Off. + */ + VNVPEMODE_OFF = 0, + /** \brief Mode 1. + */ + VNVPEMODE_MODE1 = 1 +} VnVpeMode; + +/** \brief Different scenario modes for the INS Basic Configuration register. */ +typedef enum +{ + /** \brief AHRS. + */ + VNSCENARIO_AHRS = 0, + /** \brief General purpose INS with barometric pressure sensor. + */ + VNSCENARIO_INSWITHPRESSURE = 1, + /** \brief General purpose INS without barometric pressure sensor. + */ + VNSCENARIO_INSWITHOUTPRESSURE = 2, + /** \brief GPS moving baseline for dynamic applications. + */ + VNSCENARIO_GPSMOVINGBASELINEDYNAMIC = 3, + /** \brief GPS moving baseline for static applications. + */ + VNSCENARIO_GPSMOVINGBASELINESTATIC = 4 +} VnScenario; + +/** \brief HSI modes used for the Magnetometer Calibration Control register. */ +typedef enum +{ + /** \brief Real-time hard/soft iron calibration algorithm is turned off. + */ + VNHSIMODE_OFF = 0, + /** \brief Runs the real-time hard/soft iron calibration algorithm. + */ + VNHSIMODE_RUN = 1, + /** \brief Resets the real-time hard/soft iron solution. + */ + VNHSIMODE_RESET = 2 +} VnHsiMode; + +/** \brief HSI output types for the Magnetometer Calibration Control register. */ +typedef enum +{ + /** \brief Onboard HSI is not applied to the magnetic measurements. + */ + VNHSIOUTPUT_NOONBOARD = 1, + /** \brief Onboard HSI is applied to the magnetic measurements. + */ + VNHSIOUTPUT_USEONBOARD = 3 +} VnHsiOutput; + +/** \brief Type of velocity compensation performed by the VPE. */ +typedef enum +{ + /** \brief Disabled + */ + VNVELOCITYCOMPENSATIONMODE_DISABLED = 0, + /** \brief Body Measurement + */ + VNVELOCITYCOMPENSATIONMODE_BODYMEASUREMENT = 1 +} VnVelocityCompensationMode; + +/** \brief How the magnetometer is used by the filter. */ +typedef enum +{ + /** \brief Magnetometer will only affect heading. + */ + VNMAGNETICMODE_2D = 0, + /** \brief Magnetometer will affect heading, pitch, and roll. + */ + VNMAGNETICMODE_3D = 1 +} VnMagneticMode; + +/** \brief Source for magnetometer used by the filter. */ +typedef enum +{ + /** \brief Use internal magnetometer. + */ + VNEXTERNALSENSORMODE_INTERNAL = 0, + /** \brief Use external magnetometer. Will use measurement at every new time step. + */ + VNEXTERNALSENSORMODE_EXTERNAL200HZ = 1, + /** \brief Use external magnetometer. Will only use when the measurement is updated. + */ + VNEXTERNALSENSORMODE_EXTERNALONUPDATE = 2 +} VnExternalSensorMode; + +/** \brief Options for the use of FOAM. */ +typedef enum +{ + /** \brief FOAM is not used. + */ + VNFOAMINIT_NOFOAMINIT = 0, + /** \brief FOAM is used to initialize only pitch and roll. + */ + VNFOAMINIT_FOAMINITPITCHROLL = 1, + /** \brief FOAM is used to initialize heading, pitch and roll. + */ + VNFOAMINIT_FOAMINITHEADINGPITCHROLL = 2, + /** \brief FOAM is used to initialize pitch, roll and covariance. + */ + VNFOAMINIT_FOAMINITPITCHROLLCOVARIANCE = 3, + /** \brief FOAM is used to initialize heading, pitch, roll and covariance + */ + VNFOAMINIT_FOAMINITHEADINGPITCHROLLCOVARIANCE = 4 +} VnFoamInit; + +/** \brief The different types of processors. */ +typedef enum +{ + VNPROCESSOR_NAV, /**< Navigation Processor. */ + VNPROCESSOR_GPS, /**< GPS Processor. */ + VNPROCESSOR_IMU /**< IMU Processor. */ +} VnProcessorType; + +#endif \ No newline at end of file diff --git a/Libs/vn-interface/include/vn-interface/frames.h b/Libs/vn-interface/include/vn-interface/frames.h new file mode 100644 index 0000000..4b722e2 --- /dev/null +++ b/Libs/vn-interface/include/vn-interface/frames.h @@ -0,0 +1,50 @@ +#ifndef _FRAMES_H_ +#define _FRAMES_H_ + +#include "registers.h" + +extern "C"{ + namespace vn{ + typedef struct QuaternionMagneticAccelerationAndAngularRates{ + static constexpr uint16_t ID = 8; + QuaternionMagneticAccelerationAndAngularRatesRegister Register; + QuaternionMagneticAccelerationAndAngularRates(){ + memset(&Register, 0, sizeof(Register)); + } + }; + + typedef struct InsSolutionLla{ + static constexpr uint16_t ID = 63; + InsSolutionLlaRegister Register; + InsSolutionLla(){ + memset(&Register, 0, sizeof(Register)); + } + }; + + typedef struct GpsSolutionLla{ + static constexpr uint16_t ID = 58; + GpsSolutionLlaRegister Register; + GpsSolutionLla(){ + memset(&Register, 0, sizeof(Register)); + } + }; + + typedef struct YawPitchRollTrueBodyAccelerationAndAngularRates{ + static constexpr uint16_t ID = 58; + YawPitchRollTrueBodyAccelerationAndAngularRatesRegister Register; + YawPitchRollTrueBodyAccelerationAndAngularRates(){ + memset(&Register, 0, sizeof(Register)); + } + }; + + typedef struct MagneticAccelerationAndAngularRates{ + static constexpr uint16_t ID = 58; + MagneticAccelerationAndAngularRatesRegister Register; + MagneticAccelerationAndAngularRates(){ + memset(&Register, 0, sizeof(Register)); + } + }; + } +} + +#endif /* _FRAMES_H_ */ \ No newline at end of file diff --git a/Libs/vn-interface/include/vn-interface/helper.h b/Libs/vn-interface/include/vn-interface/helper.h new file mode 100644 index 0000000..0285a40 --- /dev/null +++ b/Libs/vn-interface/include/vn-interface/helper.h @@ -0,0 +1,57 @@ +#ifndef _VN_HELPER_H_ +#define _VN_HELPER_H_ + +#include +#include +#include + +#define VN_SPI_READ 0x01 +#define VN_SPI_WRITE 0x02 + +namespace vn { + +const uint8_t ImuMeasurementsRegisterID = 54; + +/** \brief TODO */ +union header_t { + struct request_t { + uint8_t Cmd = 0; /* Defined in VN_SPI_READ */ + uint8_t ID = 0; /* Register ID */ + uint16_t Empty = 0; /* Spacer */ + request_t(uint8_t ID, uint8_t Cmd) { + this->ID = ID; + this->Cmd = Cmd; + }; + }; + struct response_t { + uint8_t Empty1 = 0; /* Spacer */ + uint8_t Cmd = 0; /* Defined in VN_SPI_READ */ + uint8_t ID = 0; /* Register ID */ + uint8_t Empty2 = 0; /* Spacer */ + response_t(){}; + }; +}; + +/** \brief Requests the specified register to write the payload into it */ +template struct pkg_request_write_t { + struct header_t::request_t header; + payload_t payload; + pkg_request_write_t(uint8_t ID) : header(ID, VN_SPI_WRITE){}; +}; + +/** \brief Requests the specified register to read from */ +struct pkg_request_read_t { + struct header_t::request_t header; + pkg_request_read_t(uint8_t ID) : header(ID, VN_SPI_READ){}; +}; + +/** \brief Response structure for the specified register */ +template struct pkg_response_t { + struct header_t::response_t header; + payload_t payload; + pkg_response_t() : header(){}; +}; + +} // namespace vn + +#endif diff --git a/Libs/vn-interface/include/vn-interface/int.h b/Libs/vn-interface/include/vn-interface/int.h new file mode 100644 index 0000000..c58b350 --- /dev/null +++ b/Libs/vn-interface/include/vn-interface/int.h @@ -0,0 +1,29 @@ +#ifndef VNINT_H_INCLUDED +#define VNINT_H_INCLUDED + +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable : 4820) +#pragma warning(disable : 4255) +#endif + +/* Visual Studio 2008 and earlier do not include the stdint.h header file. */ +#if defined(_MSC_VER) && _MSC_VER <= 1500 + 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 + /* Just include the standard header file for integer types. */ + #include +#endif + +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#endif diff --git a/Libs/vn-interface/include/vn-interface/math/matrix.h b/Libs/vn-interface/include/vn-interface/math/matrix.h new file mode 100644 index 0000000..e187748 --- /dev/null +++ b/Libs/vn-interface/include/vn-interface/math/matrix.h @@ -0,0 +1,69 @@ +#ifndef VN_MATRIX_H_INCLUDED +#define VN_MATRIX_H_INCLUDED + +#ifdef __cplusplus +extern "C" { +#endif + +/** \brief Represents a 3x3 matrix with an underlying data type of float. */ +typedef union { + float e[3 * 3]; /**< The matrix's elements in column-major ordering. */ + + struct { + float e00; /**< Element [0,0]. */ + float e10; /**< Element [1,0]. */ + float e20; /**< Element [2,0]. */ + float e01; /**< Element [0,1]. */ + float e11; /**< Element [1,1]. */ + float e21; /**< Element [2,1]. */ + float e02; /**< Element [0,2]. */ + float e12; /**< Element [1,2]. */ + float e22; /**< Element [2,2]. */ + }; + +} mat3f; + +/** \brief Represents a quaternion reading with underlying data type of float. */ +typedef union { + float c[4]; /**< Indexable. */ + + struct { + float x; /**< The x component. */ + float y; /**< The y component. */ + float z; /**< The z component. */ + float w; /**< The w component. */ + }; + + struct { + float c0; /**< Component 0. */ + float c1; /**< Component 1. */ + float c2; /**< Component 2. */ + float c3; /**< Component 2. */ + }; + +} quatf; + +/** \brief Initializes a 3x3 float matrix from an float array with matrix + * elements in column-major ordering. + * + * \param[out] m 3x3 float matrix to initialize + * \param[in] fa float array containing a 3x3 matrix in column-major order */ +void vn_m3_init_fa(mat3f *m, const float *fa); + +/** \brief Converts a mat3f to a string. + * + * \param[out] out The char buffer to output the result to. + * \param[in] m The mat3f to convert. + */ +void strFromMat3f(char *out, mat3f m); + +/** \brief Negates a mat3f. + * \param[in] m Matrix to negate. + * \return Negated matrix. */ +mat3f vnm_negative_mat3f(mat3f m); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/Libs/vn-interface/include/vn-interface/math/vector.h b/Libs/vn-interface/include/vn-interface/math/vector.h new file mode 100644 index 0000000..6a62433 --- /dev/null +++ b/Libs/vn-interface/include/vn-interface/math/vector.h @@ -0,0 +1,148 @@ +#ifndef VN_VECTOR_H_INCLUDED +#define VN_VECTOR_H_INCLUDED +#include +#include + +/** \brief Various vector types and operations. */ + +#ifdef __cplusplus +extern "C" { +#endif + +/** \brief Represents a 3 component vector with an underlying data type of + * float. */ +typedef union vec3f { + float c[3]; /**< Indexable. */ + + struct { + float x; /**< X component. */ + float y; /**< Y component. */ + float z; /**< Z component. */ + }; + + struct { + float c0; /**< Component 0. */ + float c1; /**< Component 1. */ + float c2; /**< Component 2. */ + }; + vec3f() { std::memset(this, 0, sizeof(vec3f)); } +}; + +/** \brief Represents a 3 component vector with an underlying data type of + * double. */ +typedef union vec3d { + double c[3]; /**< Indexable. */ + + struct { + double x; /**< The x component. */ + double y; /**< The y component. */ + double z; /**< The z component. */ + }; + + struct { + double c0; /**< Component 0. */ + double c1; /**< Component 1. */ + double c2; /**< Component 2. */ + }; + vec3d() { std::memset(this, 0, sizeof(vec3d)); } +}; + +/** \brief Represents a 4 component vector with an underlying data type of + * float. */ +typedef union vec4f { + float c[4]; /**< Indexable. */ + + struct { + float x; /**< The x component. */ + float y; /**< The y component. */ + float z; /**< The z component. */ + float w; /**< The w component. */ + }; + + struct { + float c0; /**< Component 0. */ + float c1; /**< Component 1. */ + float c2; /**< Component 2. */ + float c3; /**< Component 2. */ + }; + vec4f() { std::memset(this, 0, sizeof(vec4f)); } +}; + +/** \brief Initializes a 3-dimensional float vector from an float array. + * + * \param[out] v 3-dimensional float vector to initialize + * \param[in] fa float array a 3-componet vector */ +void vn_v3_init_fa(vec3f *v, const float *fa); + +/** Creates a vec3d initialized with provided values. + * \param[in] x x-component. + * \param[in] y y-component. + * \param[in] z z-component. + * \return The initialized vec3d. */ +vec3d create_v3d(double x, double y, double z); + +/** \brief Adds two vec3f together. + * + * \param[in] lhs The lhs vec3f. + * \param[in] rhs The rhs vec3f. + * \return The resulting vec3f from adding lhs and rhs together. */ +vec3f add_v3f_v3f(vec3f lhs, vec3f rhs); + +/** \brief Adds two vec3d together. + * + * \param[in] lhs The lhs vec3d. + * \param[in] rhs The rhs vec3d. + * \return The resulting vec3d from adding lhs and rhs together. */ +vec3d add_v3d_v3d(vec3d lhs, vec3d rhs); + +/** \brief Adds two vec4f together. + * + * \param[in] lhs The lhs vec4f. + * \param[in] rhs The rhs vec4f. + * \return The resulting vec4f from adding lhs and rhs together. */ +vec4f add_v4f_v4f(vec4f lhs, vec4f rhs); + +/** \brief Subtracts a vec3f from another vec3f. + * + * \param[in] lhs The lhs vec3f. + * \param[in] rhs The rhs vec3f. + * \return The resulting vec3f from subtracting rhs from lhs. */ +vec3f sub_v3f_v3f(vec3f lhs, vec3f rhs); + +/** \brief Subtracts a vec3d from another vec3d. + * + * \param[in] lhs The lhs vec3d. + * \param[in] rhs The rhs vec3d. + * \return The resulting vec3d from subtracting rhs from lhs. */ +vec3d sub_v3d_v3d(vec3d lhs, vec3d rhs); + +/** \brief Subtracts a vec4f from another vec4f. + * + * \param[in] lhs The lhs vec4f. + * \param[in] rhs The rhs vec4f. + * \return The resulting vec4f from subtracting rhs from lhs. */ +vec4f sub_v4f_v4f(vec4f lhs, vec4f rhs); + +/** \brief Converts a vec3f to a string. + * + * \param[out] out The char buffer to output the result to. + * \param[in] v The vec3f to convert. */ +void str_vec3f(char *out, vec3f v); + +/** \brief Converts a vec3d to a string. + * + * \param[out] out The char buffer to output the result to. + * \param[in] v The vec3d to convert. */ +void str_vec3d(char *out, vec3d v); + +/** \brief Converts a vec4f to a string. + * + * \param[out] out The char buffer to output the result to. + * \param[in] v The vec4f to convert. */ +void str_vec4f(char *out, vec4f v); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/Libs/vn-interface/include/vn-interface/registers.h b/Libs/vn-interface/include/vn-interface/registers.h new file mode 100644 index 0000000..33090b6 --- /dev/null +++ b/Libs/vn-interface/include/vn-interface/registers.h @@ -0,0 +1,845 @@ +/** \file + * {COMMON_HEADER} + * + * \section Description + * This header file contains declarations for using VectorNav sensors. + */ +#ifndef _VNSENSORS_H_ +#define _VNSENSORS_H_ + +#include +#include + +#include "enum.h" +#include "int.h" +#include "math/matrix.h" +#include "math/vector.h" + +#ifdef __cplusplus +extern "C" { +namespace vn { +#endif + +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable : 4820) +#endif + +// Define the max record size of a firmware update file +#define MAXFIRMWAREUPDATERECORDSIZE 300 + +/** \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. */ +typedef struct { + AsyncMode asyncMode; /**< The asyncMode field. */ + uint16_t rateDivisor; /**< The rateDivisor field. */ + CommonGroup commonField; /**< Group 1 (Common) */ + TimeGroup timeField; /**< Group 2 (Time) */ + ImuGroup imuField; /**< Group 3 (IMU) */ + GpsGroup gpsField; /**< Group 4 (GPS) */ + AttitudeGroup attitudeField; /**< Group 5 (Attitude) */ + InsGroup insField; /**< Group 6 (INS) */ + GpsGroup gps2Field; /**< Group 7 (GPS2) */ +} BinaryOutputRegister; + +/** \brief Initializes a BinaryOutputRegister structure. + * + * \param[in] reg The BinaryOutputRegister structure to initialize. + * \param[in] asyncMode Value to initialize the asyncMode field with. + * \param[in] rateDivisor Value to initialize the rateDivisor field with. + * \param[in] commonField Value to initialize the commonField with. + * \param[in] timeField Value to initialize the timeField with. + * \param[in] imuField Value to initialize the imuField with. + * \param[in] gpsField Value to initialize the gpsField with. + * \param[in] attitudeField Value to initialize the attitudeField with. + * \param[in] insField Value to initialize the insField with. */ +void BinaryOutputRegister_initialize(BinaryOutputRegister *reg, AsyncMode asyncMode, uint32_t rateDivisor, CommonGroup commonField, + TimeGroup timeField, ImuGroup imuField, GpsGroup gpsField, AttitudeGroup attitudeField, InsGroup insField, + GpsGroup gps2Field); + +/** \brief Structure representing the Quaternion, Magnetic, Acceleration and Angular Rates register. */ +typedef struct { + /** \brief The Quat field. */ + vec4f quat; + + /** \brief The Mag field. */ + vec3f mag; + + /** \brief The Accel field. */ + vec3f accel; + + /** \brief The Gyro field. */ + vec3f gyro; +} QuaternionMagneticAccelerationAndAngularRatesRegister; + +/** \brief Structure representing the Magnetic, Acceleration and Angular Rates register. */ +typedef struct { + /** \brief The Mag field. */ + vec3f mag; + + /** \brief The Accel field. */ + vec3f accel; + + /** \brief The Gyro field. */ + vec3f gyro; + +} MagneticAccelerationAndAngularRatesRegister; + +/** \brief Structure representing the Magnetic and Gravity Reference Vectors register. */ +typedef struct { + /** \brief The MagRef field. */ + vec3f magRef; + + /** \brief The AccRef field. */ + vec3f accRef; + +} MagneticAndGravityReferenceVectorsRegister; + +/** \brief Structure representing the Filter Measurements Variance Parameters register. */ +typedef struct { + /** \brief The Angular Walk Variance field. */ + float angularWalkVariance; + + /** \brief The Angular Rate Variance field. */ + vec3f angularRateVariance; + + /** \brief The Magnetic Variance field. */ + vec3f magneticVariance; + + /** \brief The Acceleration Variance field. */ + vec3f accelerationVariance; + +} FilterMeasurementsVarianceParametersRegister; + +/** \brief Structure representing the Magnetometer Compensation register. */ +typedef struct { + /** \brief The C field. */ + mat3f c; + + /** \brief The B field. */ + vec3f b; + +} MagnetometerCompensationRegister; + +/** \brief Structure representing the Filter Active Tuning Parameters register. */ +typedef struct { + /** \brief The Magnetic Disturbance Gain field. */ + float magneticDisturbanceGain; + + /** \brief The Acceleration Disturbance Gain field. */ + float accelerationDisturbanceGain; + + /** \brief The Magnetic Disturbance Memory field. */ + float magneticDisturbanceMemory; + + /** \brief The Acceleration Disturbance Memory field. */ + float accelerationDisturbanceMemory; + +} FilterActiveTuningParametersRegister; + +/** \brief Structure representing the Acceleration Compensation register. */ +typedef struct { + /** \brief The C field. */ + mat3f c; + + /** \brief The B field. */ + vec3f b; + +} AccelerationCompensationRegister; + +/** \brief Structure representing the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register. */ +typedef struct { + /** \brief The YawPitchRoll field. */ + vec3f yawPitchRoll; + + /** \brief The Mag field. */ + vec3f mag; + + /** \brief The Accel field. */ + vec3f accel; + + /** \brief The Gyro field. */ + vec3f gyro; + +} YawPitchRollMagneticAccelerationAndAngularRatesRegister; + +/** \brief Structure representing the Communication Protocol Control register. */ +typedef struct { + /** \brief The SerialCount field. */ + uint8_t serialCount; + + /** \brief The SerialStatus field. */ + uint8_t serialStatus; + + /** \brief The SPICount field. */ + uint8_t spiCount; + + /** \brief The SPIStatus field. */ + uint8_t spiStatus; + + /** \brief The SerialChecksum field. */ + uint8_t serialChecksum; + + /** \brief The SPIChecksum field. */ + uint8_t spiChecksum; + + /** \brief The ErrorMode field. */ + uint8_t errorMode; + +} CommunicationProtocolControlRegister; + +/** \brief Structure representing the Synchronization Control register. */ +typedef struct { + /** \brief The SyncInMode field. */ + uint8_t syncInMode; + + /** \brief The SyncInEdge field. */ + uint8_t syncInEdge; + + /** \brief The SyncInSkipFactor field. */ + uint16_t syncInSkipFactor; + + /** \brief The SyncOutMode field. */ + uint8_t syncOutMode; + + /** \brief The SyncOutPolarity field. */ + uint8_t syncOutPolarity; + + /** \brief The SyncOutSkipFactor field. */ + uint16_t syncOutSkipFactor; + + /** \brief The SyncOutPulseWidth field. */ + uint32_t syncOutPulseWidth; + +} SynchronizationControlRegister; + +/** \brief Structure representing the Synchronization Status register. */ +typedef struct { + /** \brief The SyncInCount field. */ + uint32_t syncInCount; + + /** \brief The SyncInTime field. */ + uint32_t syncInTime; + + /** \brief The SyncOutCount field. */ + uint32_t syncOutCount; + +} SynchronizationStatusRegister; + +/** \brief Structure representing the Filter Basic Control register. */ +typedef struct { + /** \brief The MagMode field. */ + uint8_t magMode; + + /** \brief The ExtMagMode field. */ + uint8_t extMagMode; + + /** \brief The ExtAccMode field. */ + uint8_t extAccMode; + + /** \brief The ExtGyroMode field. */ + uint8_t extGyroMode; + + /** \brief The GyroLimit field. */ + vec3f gyroLimit; + +} FilterBasicControlRegister; + +/** \brief Structure representing the VPE Basic Control register. */ +typedef struct { + /** \brief The Enable field. */ + uint8_t enable; + + /** \brief The HeadingMode field. */ + uint8_t headingMode; + + /** \brief The FilteringMode field. */ + uint8_t filteringMode; + + /** \brief The TuningMode field. */ + uint8_t tuningMode; + +} VpeBasicControlRegister; + +/** \brief Structure representing the VPE Magnetometer Basic Tuning register. */ +typedef struct { + /** \brief The BaseTuning field. */ + vec3f baseTuning; + + /** \brief The AdaptiveTuning field. */ + vec3f adaptiveTuning; + + /** \brief The AdaptiveFiltering field. */ + vec3f adaptiveFiltering; + +} VpeMagnetometerBasicTuningRegister; + +/** \brief Structure representing the VPE Magnetometer Advanced Tuning register. */ +typedef struct { + /** \brief The MinFiltering field. */ + vec3f minFiltering; + + /** \brief The MaxFiltering field. */ + vec3f maxFiltering; + + /** \brief The MaxAdaptRate field. */ + float maxAdaptRate; + + /** \brief The DisturbanceWindow field. */ + float disturbanceWindow; + + /** \brief The MaxTuning field. */ + float maxTuning; + +} VpeMagnetometerAdvancedTuningRegister; + +/** \brief Structure representing the VPE Accelerometer Basic Tuning register. */ +typedef struct { + /** \brief The BaseTuning field. */ + vec3f baseTuning; + + /** \brief The AdaptiveTuning field. */ + vec3f adaptiveTuning; + + /** \brief The AdaptiveFiltering field. */ + vec3f adaptiveFiltering; + +} VpeAccelerometerBasicTuningRegister; + +/** \brief Structure representing the VPE Accelerometer Advanced Tuning register. */ +typedef struct { + /** \brief The MinFiltering field. */ + vec3f minFiltering; + + /** \brief The MaxFiltering field. */ + vec3f maxFiltering; + + /** \brief The MaxAdaptRate field. */ + float maxAdaptRate; + + /** \brief The DisturbanceWindow field. */ + float disturbanceWindow; + + /** \brief The MaxTuning field. */ + float maxTuning; + +} VpeAccelerometerAdvancedTuningRegister; + +/** \brief Structure representing the VPE Gyro Basic Tuning register. */ +typedef struct { + /** \brief The AngularWalkVariance field. */ + vec3f angularWalkVariance; + + /** \brief The BaseTuning field. */ + vec3f baseTuning; + + /** \brief The AdaptiveTuning field. */ + vec3f adaptiveTuning; + +} VpeGyroBasicTuningRegister; + +/** \brief Structure representing the Magnetometer Calibration Control register. */ +typedef struct { + /** \brief The HSIMode field. */ + uint8_t hsiMode; + + /** \brief The HSIOutput field. */ + uint8_t hsiOutput; + + /** \brief The ConvergeRate field. */ + uint8_t convergeRate; + +} MagnetometerCalibrationControlRegister; + +/** \brief Structure representing the Calculated Magnetometer Calibration register. */ +typedef struct { + /** \brief The C field. */ + mat3f c; + + /** \brief The B field. */ + vec3f b; + +} CalculatedMagnetometerCalibrationRegister; + +/** \brief Structure representing the Velocity Compensation Control register. */ +typedef struct { + /** \brief The Mode field. */ + uint8_t mode; + + /** \brief The VelocityTuning field. */ + float velocityTuning; + + /** \brief The RateTuning field. */ + float rateTuning; + +} VelocityCompensationControlRegister; + +/** \brief Structure representing the Velocity Compensation Status register. */ +typedef struct { + /** \brief The x field. */ + float x; + + /** \brief The xDot field. */ + float xDot; + + /** \brief The accelOffset field. */ + vec3f accelOffset; + + /** \brief The omega field. */ + vec3f omega; + +} VelocityCompensationStatusRegister; + +/** \brief Structure representing the IMU Measurements register. */ +typedef struct { + /** \brief The Mag field. */ + vec3f mag; + + /** \brief The Accel field. */ + vec3f accel; + + /** \brief The Gyro field. */ + vec3f gyro; + + /** \brief The Temp field. */ + float temp; + + /** \brief The Pressure field. */ + float pressure; +} ImuMeasurementsRegister; + +/** \brief Structure representing the GPS Configuration register. */ +typedef struct { + /** \brief The Mode field. */ + uint8_t mode; + + /** \brief The PpsSource field. */ + uint8_t ppsSource; + +} GpsConfigurationRegister; + +/** \brief Structure representing the GPS Solution - LLA register. */ +typedef struct { + /** \brief The Time field. */ + double time; + + /** \brief The Week field. */ + uint16_t week; + + /** \brief The GpsFix field. */ + uint8_t gpsFix; + + /** \brief The NumSats field. */ + uint8_t numSats; + + /** \brief The Lla field. */ + vec3d lla; + + /** \brief The NedVel field. */ + vec3f nedVel; + + /** \brief The NedAcc field. */ + vec3f nedAcc; + + /** \brief The SpeedAcc field. */ + float speedAcc; + + /** \brief The TimeAcc field. */ + float timeAcc; +} GpsSolutionLlaRegister; + +/** \brief Structure representing the GPS Solution - ECEF register. */ +typedef struct { + /** \brief The Tow field. */ + double tow; + + /** \brief The Week field. */ + uint16_t week; + + /** \brief The GpsFix field. */ + uint8_t gpsFix; + + /** \brief The NumSats field. */ + uint8_t numSats; + + /** \brief The Position field. */ + vec3d position; + + /** \brief The Velocity field. */ + vec3f velocity; + + /** \brief The PosAcc field. */ + vec3f posAcc; + + /** \brief The SpeedAcc field. */ + float speedAcc; + + /** \brief The TimeAcc field. */ + float timeAcc; + +} GpsSolutionEcefRegister; + +typedef struct InsSolutionLlaRegister { +/** \brief Structure representing the INS Solution - LLA register. */ + /** \brief The Time field. */ + double time; + + /** \brief The Week field. */ + uint16_t week; + + /** \brief The Status field. */ + uint16_t status; + + /** \brief The YawPitchRoll field. */ + vec3f yawPitchRoll; + + /** \brief The Position field. */ + vec3d position; + + /** \brief The NedVel field. */ + vec3f nedVel; + + /** \brief The AttUncertainty field. */ + float attUncertainty; + + /** \brief The PosUncertainty field. */ + float posUncertainty; + + /** \brief The VelUncertainty field. */ + float velUncertainty; +}; + +/** \brief Structure representing the INS Solution - ECEF register. */ +typedef struct { + + /** \brief The Time field. */ + double time; + + /** \brief The Week field. */ + uint16_t week; + + /** \brief The Status field. */ + uint16_t status; + + /** \brief The YawPitchRoll field. */ + vec3f yawPitchRoll; + + /** \brief The Position field. */ + vec3d position; + + /** \brief The Velocity field. */ + vec3f velocity; + + /** \brief The AttUncertainty field. */ + float attUncertainty; + + /** \brief The PosUncertainty field. */ + float posUncertainty; + + /** \brief The VelUncertainty field. */ + float velUncertainty; +} InsSolutionEcefRegister; + +/** \brief Structure representing the INS Basic Configuration register for a VN-200 sensor. */ +typedef struct { + /** \brief The Scenario field. */ + uint8_t scenario; + + /** \brief The AhrsAiding field. */ + uint8_t ahrsAiding; + +} InsBasicConfigurationRegisterVn200; + +/** \brief Structure representing the INS Basic Configuration register for a VN-300 sensor. */ +typedef struct { + /** \brief The Scenario field. */ + uint8_t scenario; + + /** \brief The AhrsAiding field. */ + uint8_t ahrsAiding; + + /** \brief The EstBaseline field. */ + uint8_t estBaseline; + +} InsBasicConfigurationRegisterVn300; + +/** \brief Structure representing the INS Advanced Configuration register. */ +typedef struct { + /** \brief The UseMag field. */ + uint8_t useMag; + + /** \brief The UsePres field. */ + uint8_t usePres; + + /** \brief The PosAtt field. */ + uint8_t posAtt; + + /** \brief The VelAtt field. */ + uint8_t velAtt; + + /** \brief The VelBias field. */ + uint8_t velBias; + + /** \brief The UseFoam field. */ + uint8_t useFoam; + + /** \brief The GPSCovType field. */ + uint8_t gpsCovType; + + /** \brief The VelCount field. */ + uint8_t velCount; + + /** \brief The VelInit field. */ + float velInit; + + /** \brief The MoveOrigin field. */ + float moveOrigin; + + /** \brief The GPSTimeout field. */ + float gpsTimeout; + + /** \brief The DeltaLimitPos field. */ + float deltaLimitPos; + + /** \brief The DeltaLimitVel field. */ + float deltaLimitVel; + + /** \brief The MinPosUncertainty field. */ + float minPosUncertainty; + + /** \brief The MinVelUncertainty field. */ + float minVelUncertainty; + +} InsAdvancedConfigurationRegister; + +/** \brief Structure representing the INS State - LLA register. */ +typedef struct { + /** \brief The YawPitchRoll field. */ + vec3f yawPitchRoll; + + /** \brief The Position field. */ + vec3d position; + + /** \brief The Velocity field. */ + vec3f velocity; + + /** \brief The Accel field. */ + vec3f accel; + + /** \brief The AngularRate field. */ + vec3f angularRate; + +} InsStateLlaRegister; + +/** \brief Structure representing the INS State - ECEF register. */ +typedef struct { + /** \brief The YawPitchRoll field. */ + vec3f yawPitchRoll; + + /** \brief The Position field. */ + vec3d position; + + /** \brief The Velocity field. */ + vec3f velocity; + + /** \brief The Accel field. */ + vec3f accel; + + /** \brief The AngularRate field. */ + vec3f angularRate; + +} InsStateEcefRegister; + +/** \brief Structure representing the Startup Filter Bias Estimate register. */ +typedef struct { + /** \brief The GyroBias field. */ + vec3f gyroBias; + + /** \brief The AccelBias field. */ + vec3f accelBias; + + /** \brief The PressureBias field. */ + float pressureBias; + +} StartupFilterBiasEstimateRegister; + +/** \brief Structure representing the Delta Theta and Delta Velocity register. */ +typedef struct { + /** \brief The DeltaTime field. */ + float deltaTime; + + /** \brief The DeltaTheta field. */ + vec3f deltaTheta; + + /** \brief The DeltaVelocity field. */ + vec3f deltaVelocity; + +} DeltaThetaAndDeltaVelocityRegister; + +/** \brief Structure representing the Delta Theta and Delta Velocity Configuration register. */ +typedef struct { + /** \brief The IntegrationFrame field. */ + uint8_t integrationFrame; + + /** \brief The GyroCompensation field. */ + uint8_t gyroCompensation; + + /** \brief The AccelCompensation field. */ + uint8_t accelCompensation; + +} DeltaThetaAndDeltaVelocityConfigurationRegister; + +/** \brief Structure representing the Reference Vector Configuration register. */ +typedef struct { + /** \brief The UseMagModel field. */ + uint8_t useMagModel; + + /** \brief The UseGravityModel field. */ + uint8_t useGravityModel; + + /** \brief The RecalcThreshold field. */ + uint32_t recalcThreshold; + + /** \brief The Year field. */ + float year; + + /** \brief The Position field. */ + vec3d position; + +} ReferenceVectorConfigurationRegister; + +/** \brief Structure representing the Gyro Compensation register. */ +typedef struct { + /** \brief The C field. */ + mat3f c; + + /** \brief The B field. */ + vec3f b; + +} GyroCompensationRegister; + +/** \brief Structure representing the IMU Filtering Configuration register. */ +typedef struct { + /** \brief The MagWindowSize field. */ + uint16_t magWindowSize; + + /** \brief The AccelWindowSize field. */ + uint16_t accelWindowSize; + + /** \brief The GyroWindowSize field. */ + uint16_t gyroWindowSize; + + /** \brief The TempWindowSize field. */ + uint16_t tempWindowSize; + + /** \brief The PresWindowSize field. */ + uint16_t presWindowSize; + + /** \brief The MagFilterMode field. */ + uint8_t magFilterMode; + + /** \brief The AccelFilterMode field. */ + uint8_t accelFilterMode; + + /** \brief The GyroFilterMode field. */ + uint8_t gyroFilterMode; + + /** \brief The TempFilterMode field. */ + uint8_t tempFilterMode; + + /** \brief The PresFilterMode field. */ + uint8_t presFilterMode; + +} ImuFilteringConfigurationRegister; + +/** \brief Structure representing the GPS Compass Baseline register. */ +typedef struct { + /** \brief The Position field. */ + vec3f position; + + /** \brief The Uncertainty field. */ + vec3f uncertainty; + +} GpsCompassBaselineRegister; + +/** \brief Structure representing the GPS Compass Estimated Baseline register. */ +typedef struct { + /** \brief The EstBaselineUsed field. */ + uint8_t estBaselineUsed; + + /** \brief The NumMeas field. */ + uint16_t numMeas; + + /** \brief The Position field. */ + vec3f position; + + /** \brief The Uncertainty field. */ + vec3f uncertainty; + +} GpsCompassEstimatedBaselineRegister; + +/** \brief Structure representing the IMU Rate Configuration register. */ +typedef struct { + /** \brief The imuRate field. */ + uint16_t imuRate; + + /** \brief The NavDivisor field. */ + uint16_t navDivisor; + + /** \brief The filterTargetRate field. */ + float filterTargetRate; + + /** \brief The filterMinRate field. */ + float filterMinRate; + +} ImuRateConfigurationRegister; + + + +/** \brief Structure representing the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register. */ +typedef struct YawPitchRollTrueBodyAccelerationAndAngularRatesRegister { + + /** \brief The YawPitchRoll field. */ + vec3f yawPitchRoll; + + /** \brief The BodyAccel field. */ + vec3f bodyAccel; + + /** \brief The Gyro field. */ + vec3f gyro; + +}; + +/** \brief Structure representing the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register. */ +typedef struct { + /** \brief The YawPitchRoll field. */ + vec3f yawPitchRoll; + + /** \brief The InertialAccel field. */ + vec3f inertialAccel; + + /** \brief The Gyro field. */ + vec3f gyro; + +} YawPitchRollTrueInertialAccelerationAndAngularRatesRegister; + +#ifdef __cplusplus +} +} +#endif + +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#endif diff --git a/Libs/vn-interface/include/vn-interface/types.h b/Libs/vn-interface/include/vn-interface/types.h new file mode 100644 index 0000000..27aed1e --- /dev/null +++ b/Libs/vn-interface/include/vn-interface/types.h @@ -0,0 +1,56 @@ +#ifndef VN_TYPES_H_INCLUDED +#define VN_TYPES_H_INCLUDED + +/** \brief Standard types used through out the library. */ +#include "int.h" + +#if !defined(__cplusplus) + +#if (defined(__STDC_VERSION__) && __STDC_VERSION__ >= 199901L) || defined(__GNUC__) + +#include + +#else + +/* Must not have C99. */ + +/** Backup definition of size_t. */ +typedef unsigned int size_t; + +#endif + +#else +extern "C" { +#endif + +/* Adding VectorNav data types */ +typedef struct { + int8_t year; /** \brief Year field. */ + uint8_t month; /** \brief Month field. */ + uint8_t day; /** \brief Day field. */ + uint8_t hour; /** \brief Hour field. */ + uint8_t min; /** \brief Min field. */ + uint8_t sec; /** \brief Sec field. */ + uint16_t ms; /** \brief Ms field. */ +} TimeUtc; + +typedef struct { + float gDOP; /** \brief Gdop field. */ + float pDOP; /** \brief Pdop field. */ + float tDOP; /** \brief Tdop field. */ + float vDOP; /** \brief Vdop field. */ + float hDOP; /** \brief Hdop field. */ + float nDOP; /** \brief Ndop field. */ + float eDOP; /** \brief Edop field. */ +} GpsDop; + +typedef struct { + uint8_t timeStatus; /** \brief timeStatus field. */ + int8_t leapSeconds; /** \brief leapSeconds field. */ +} TimeInfo; + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/Libs/vn-interface/src/helper.cpp b/Libs/vn-interface/src/helper.cpp new file mode 100644 index 0000000..2f88d80 --- /dev/null +++ b/Libs/vn-interface/src/helper.cpp @@ -0,0 +1,3 @@ +#include "vn-interface/helper.h" + +namespace vn {} // namespace vn diff --git a/Libs/vn-interface/src/math/matrix.c b/Libs/vn-interface/src/math/matrix.c new file mode 100644 index 0000000..9de581e --- /dev/null +++ b/Libs/vn-interface/src/math/matrix.c @@ -0,0 +1,20 @@ +#include "vn-interface/math/matrix.h" + +#include "vn-interface/types.h" + +void vn_m3_init_fa(mat3f *m, const float *fa) { + size_t i; + + for (i = 0; i < 9; i++) + m->e[i] = fa[i]; +} + +mat3f vnm_negative_mat3f(mat3f m) { + mat3f r; + size_t i; + + for (i = 0; i < 3 * 3; i++) + r.e[i] = -m.e[i]; + + return r; +} diff --git a/Libs/vn-interface/src/math/vector.c b/Libs/vn-interface/src/math/vector.c new file mode 100644 index 0000000..c03bea8 --- /dev/null +++ b/Libs/vn-interface/src/math/vector.c @@ -0,0 +1,118 @@ +#include "vn/math/vector.h" + +#include + +vec3d create_v3d(double x, double y, double z) +{ + vec3d v; + + v.c[0] = x; + v.c[1] = y; + v.c[2] = z; + + return v; +} + +void vn_v3_init_fa(vec3f* v, const float* fa) +{ + size_t i; + + for (i = 0; i < 3; i++) + v->c[i] = fa[i]; +} + +vec3f add_v3f_v3f(vec3f lhs, vec3f rhs) +{ + vec3f r; + + r.c[0] = lhs.c[0] + rhs.c[0]; + r.c[1] = lhs.c[1] + rhs.c[1]; + r.c[2] = lhs.c[2] + rhs.c[2]; + + return r; +} + +vec3d add_v3d_v3d(vec3d lhs, vec3d rhs) +{ + vec3d r; + + r.c[0] = lhs.c[0] + rhs.c[0]; + r.c[1] = lhs.c[1] + rhs.c[1]; + r.c[2] = lhs.c[2] + rhs.c[2]; + + return r; +} + +vec4f add_v4f_v4f(vec4f lhs, vec4f rhs) +{ + vec4f r; + + r.c[0] = lhs.c[0] + rhs.c[0]; + r.c[1] = lhs.c[1] + rhs.c[1]; + r.c[2] = lhs.c[2] + rhs.c[2]; + r.c[3] = lhs.c[3] + rhs.c[3]; + + return r; +} + +vec3f sub_v3f_v3f(vec3f lhs, vec3f rhs) +{ + vec3f r; + + r.c[0] = lhs.c[0] - rhs.c[0]; + r.c[1] = lhs.c[1] - rhs.c[1]; + r.c[2] = lhs.c[2] - rhs.c[2]; + + return r; +} + +vec3d sub_v3d_v3d(vec3d lhs, vec3d rhs) +{ + vec3d r; + + r.c[0] = lhs.c[0] - rhs.c[0]; + r.c[1] = lhs.c[1] - rhs.c[1]; + r.c[2] = lhs.c[2] - rhs.c[2]; + + return r; +} + +vec4f sub_v4f_v4f(vec4f lhs, vec4f rhs) +{ + vec4f r; + + r.c[0] = lhs.c[0] - rhs.c[0]; + r.c[1] = lhs.c[1] - rhs.c[1]; + r.c[2] = lhs.c[2] - rhs.c[2]; + r.c[3] = lhs.c[3] - rhs.c[3]; + + return r; +} + +#if defined(_MSC_VER) + /* Disable warnings regarding using sprintf_s since these + * function signatures do not provide us with information + * about the length of 'out'. */ + #pragma warning(push) + #pragma warning(disable:4996) +#endif + +void str_vec3f(char* out, vec3f v) +{ + sprintf(out, "(%f; %f; %f)", v.c[0], v.c[1], v.c[2]); +} + +void str_vec3d(char* out, vec3d v) +{ + sprintf(out, "(%f; %f; %f)", v.c[0], v.c[1], v.c[2]); +} + +void str_vec4f(char* out, vec4f v) +{ + sprintf(out, "(%f; %f; %f; %f)", v.c[0], v.c[1], v.c[2], v.c[3]); +} + + +#if defined(_MSC_VER) + #pragma warning(pop) +#endif diff --git a/Libs/vn-interface/src/registers.c b/Libs/vn-interface/src/registers.c new file mode 100644 index 0000000..8385439 --- /dev/null +++ b/Libs/vn-interface/src/registers.c @@ -0,0 +1,15 @@ +#include "vn-interface/registers.h" + +void BinaryOutputRegister_initialize(BinaryOutputRegister *reg, AsyncMode asyncMode, uint32_t rateDivisor, CommonGroup commonField, + TimeGroup timeField, ImuGroup imuField, GpsGroup gpsField, AttitudeGroup attitudeField, InsGroup insField, + GpsGroup gps2Field) { + reg->asyncMode = asyncMode; + reg->rateDivisor = (uint16_t)rateDivisor; + reg->commonField = commonField; + reg->timeField = timeField; + reg->imuField = imuField; + reg->gpsField = gpsField; + reg->attitudeField = attitudeField; + reg->insField = insField; + reg->gps2Field = gps2Field; +}