Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-10 14:51:15 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

drivers: initial VectorNav (VN-100, VN-200, VN-300) support

Browse Source
This commit is contained in:
Daniel Agar
2023-01-20 19:09:30 -05:00
committed by GitHub
parent bd9d09663f
commit 1aa8ec4537
61 changed files with 27126 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/astyle/files_to_check_code_style.sh
+1
View File
@@ -10,6 +10,7 @@ fi
exec find boards msg src platforms test \
-path platforms/nuttx/NuttX -prune -o \
-path platforms/qurt/dspal -prune -o \
-path src/drivers/ins/vectornav/libvnc -prune -o \
-path src/drivers/uavcan/libuavcan -prune -o \
-path src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis -prune -o \
-path src/drivers/cyphal/libcanard -prune -o \
Tools/px4moduledoc/srcparser.py
+1 -1
View File
@@ -15,7 +15,7 @@ class ModuleDocumentation(object):
# TOC in https://github.com/PX4/PX4-user_guide/blob/main/en/SUMMARY.md
valid_categories = ['driver', 'estimator', 'controller', 'system',
'communication', 'command', 'template', 'simulation', 'autotune']
valid_subcategories = ['', 'distance_sensor', 'imu', 'airspeed_sensor',
valid_subcategories = ['', 'distance_sensor', 'imu', 'ins', 'airspeed_sensor',
'magnetometer', 'baro', 'optical_flow', 'rpm_sensor', 'transponder']
max_line_length = 80 # wrap lines that are longer than this
boards/px4/fmu-v6c/default.px4board
+2 -1
View File
@@ -6,7 +6,7 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS5"
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3"
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BAROMETER_MS5611=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
@@ -17,6 +17,7 @@ CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_HEATER=y
CONFIG_DRIVERS_IMU_BOSCH_BMI055=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
CONFIG_DRIVERS_INS_VECTORNAV=y
CONFIG_COMMON_LIGHT=y
CONFIG_COMMON_MAGNETOMETER=y
CONFIG_COMMON_OPTICAL_FLOW=y
msg/VehicleAttitude.msg
+1 -1
View File
@@ -8,5 +8,5 @@ float32[4] q # Quaternion rotation from the FRD body frame to
float32[4] delta_q_reset # Amount by which quaternion has changed during last reset
uint8 quat_reset_counter # Quaternion reset counter
# TOPICS vehicle_attitude vehicle_attitude_groundtruth
# TOPICS vehicle_attitude vehicle_attitude_groundtruth external_ins_attitude
# TOPICS estimator_attitude
msg/VehicleGlobalPosition.msg
+1 -1
View File
@@ -25,5 +25,5 @@ bool terrain_alt_valid # Terrain altitude estimate is valid
bool dead_reckoning # True if this position is estimated through dead-reckoning
# TOPICS vehicle_global_position vehicle_global_position_groundtruth
# TOPICS vehicle_global_position vehicle_global_position_groundtruth external_ins_global_position
# TOPICS estimator_global_position
msg/VehicleLocalPosition.msg
+1 -1
View File
@@ -72,5 +72,5 @@ float32 vz_max # maximum vertical speed - set to 0 when limiting not required
float32 hagl_min # minimum height above ground level - set to 0 when limiting not required (meters)
float32 hagl_max # maximum height above ground level - set to 0 when limiting not required (meters)
# TOPICS vehicle_local_position vehicle_local_position_groundtruth
# TOPICS vehicle_local_position vehicle_local_position_groundtruth external_ins_local_position
# TOPICS estimator_local_position
src/drivers/drv_sensor.h
+5 -1
View File
@@ -222,7 +222,11 @@
#define DRV_POWER_DEVTYPE_INA220 0xD3
#define DRV_POWER_DEVTYPE_INA238 0xD4
#define DRV_DIST_DEVTYPE_TF02PRO 0xE0
#define DRV_DIST_DEVTYPE_TF02PRO 0xE0
#define DRV_INS_DEVTYPE_VN100 0xE1
#define DRV_INS_DEVTYPE_VN200 0xE2
#define DRV_INS_DEVTYPE_VN300 0xE3
#define DRV_DEVTYPE_UNUSED 0xff
src/drivers/ins/Kconfig
+9
View File
@@ -0,0 +1,9 @@
menu "Inertial Navigation Systems (INS)"
menuconfig COMMON_INS
bool "All INS sensors"
default n
select DRIVERS_INS_VECTORNAV
---help---
Enable default set of INS sensors
rsource "*/Kconfig"
endmenu
src/drivers/ins/vectornav/CMakeLists.txt
+50
View File
@@ -0,0 +1,50 @@
############################################################################
#
# Copyright (c) 2022 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
add_subdirectory(libvnc)
px4_add_module(
MODULE drivers__ins__vectornav
MAIN vectornav
INCLUDES
libvnc/include
COMPILE_FLAGS
-Wno-error
SRCS
VectorNav.cpp
VectorNav.hpp
MODULE_CONFIG
module.yaml
DEPENDS
libvnc
)
src/drivers/ins/vectornav/Kconfig
+5
View File
@@ -0,0 +1,5 @@
menuconfig DRIVERS_INS_VECTORNAV
bool "vectornav"
default n
---help---
Enable support for vectornav
src/drivers/ins/vectornav/VectorNav.cpp
+701
View File
File diff suppressed because it is too large Load Diff
src/drivers/ins/vectornav/VectorNav.hpp
+157
View File
@@ -0,0 +1,157 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
*
* Driver for the VectorNav VN-100, VN-200, VN-300 series
*/
#pragma once
#include <stdio.h>
#include <inttypes.h>
#include <termios.h>
extern "C" {
#include "vn/sensors/searcher.h"
#include "vn/sensors/compositedata.h"
}
#include "vn/sensors.h"
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <drivers/drv_hrt.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_odometry.h>
using namespace time_literals;
class VectorNav : public ModuleBase<VectorNav>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
VectorNav(const char *port);
~VectorNav() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::print_status() */
int print_status() override;
private:
bool init();
bool configure();
void Run() override;
static void binaryAsyncMessageReceived(void *userData, VnUartPacket *packet, size_t runningIndex);
void sensorCallback(VnUartPacket *packet);
char _port[20] {};
bool _initialized{false};
bool _connected{false};
bool _configured{false};
hrt_abstime _last_read{0};
VnSensor _vs{};
BinaryOutputRegister _binary_output_group_1{};
BinaryOutputRegister _binary_output_group_2{};
BinaryOutputRegister _binary_output_group_3{};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
PX4Accelerometer _px4_accel{0};
PX4Gyroscope _px4_gyro{0};
PX4Magnetometer _px4_mag{0};
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
uORB::PublicationMulti<vehicle_attitude_s> _attitude_pub{ORB_ID(external_ins_attitude)};
uORB::PublicationMulti<vehicle_local_position_s> _local_position_pub{ORB_ID(external_ins_local_position)};
uORB::PublicationMulti<vehicle_global_position_s> _global_position_pub{ORB_ID(external_ins_global_position)};
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")};
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
// TODO: params for GNSS antenna offsets
// A
// B
// As mentioned previously, the VN-300 has a factory default baseline of {1, 0, 0} [m]. This vector represents
// the position of a point on GNSS antenna B relative to the same point on GNSS antenna A in the output
// coordinate system on the VN-300. The default output coordinate system is engraved on the top of the
// aluminum enclosure. For the factory default case, GNSS antenna B should be positioned in front of GNSS
// antenna A relative to the X-axis marked on the VN-300 enclosure as shown in the figure below. If a different
// baseline length or direction required, then you will need to write the new baseline vector and the measurement
// uncertainty to the sensor using the GNSS Compass Baseline Register.
// GNSS Antenna A Offset
// Relative position of GNSS antenna. (X-axis)
// VN_GNSS_ANTA_POS_X
// Relative position of GNSS antenna. (X-axis)
// VN_GNSS_ANTB_POS_X
// Uncertainty in the X-axis position measurement.
};
src/drivers/ins/vectornav/libvnc/CMakeLists.txt
+75
View File
@@ -0,0 +1,75 @@
cmake_minimum_required(VERSION 2.8.4)
project(libvnc C)
include_directories(include)
set(SOURCES
src/vn/xplat/criticalsection.c
src/vn/xplat/event.c
src/vn/xplat/serialport.c
src/vn/xplat/thread.c
src/vn/xplat/time.c
src/vn/error.c
src/vn/error_detection.c
src/vn/util.c
src/vn/math/matrix.c
src/vn/math/vector.c
src/vn/protocol/upack.c
src/vn/protocol/upackf.c
src/vn/protocol/spi.c
src/vn/sensors/compositedata.c
src/vn/sensors/searcher.c
src/vn/sensors.c
src/vn/sensors/ezasyncdata.c
src/vn/conv.c
include/vn/vectornav.h
include/vn/bool.h
include/vn/util/compiler.h
include/vn/sensors/compositedata.h
include/vn/const.h
include/vn/conv.h
include/vn/xplat/criticalsection.h
include/vn/enum.h
include/vn/error_detection.h
include/vn/error.h
include/vn/xplat/event.h
include/vn/util/export.h
include/vn/sensors/ezasyncdata.h
include/vn/int.h
include/vn/math/matrix.h
include/vn/util/port.h
include/vn/protocol/common.h
include/vn/sensors/searcher.h
include/vn/sensors.h
include/vn/xplat/serialport.h
include/vn/protocol/spi.h
include/vn/xplat/thread.h
include/vn/xplat/time.h
include/vn/types.h
include/vn/protocol/upack.h
include/vn/protocol/upackf.h
include/vn/util.h
include/vn/math/vector.h
)
add_library(libvnc ${SOURCES})
add_dependencies(libvnc prebuild_targets)
target_compile_options(libvnc
PRIVATE
-Wno-error
-Wno-double-promotion
-Wno-pointer-sign
-Wno-cast-align
-Wno-unused-but-set-parameter
-Wno-sign-compare
-Wno-bad-function-cast
-Wno-empty-body
-Wno-switch
-Wno-unused-variable
-Wno-format
-Wno-format-security
)
if("${PX4_PLATFORM}" MATCHES "nuttx")
target_compile_definitions(libvnc PUBLIC __NUTTX__)
endif()
src/drivers/ins/vectornav/libvnc/include/vn/bool.h
+37
View File
@@ -0,0 +1,37 @@
#ifndef VNBOOL_H_INCLUDED
#define VNBOOL_H_INCLUDED
/** \brief Boolean definition. */
#include "vn/util/compiler.h"
#include "vn/int.h"
#if !defined(__cplusplus)
#if VN_HAVE_STDBOOL_H
#include <stdbool.h>
#else
#if !defined(__GNUC__)
/* _Bool builtin type is included in GCC. */
/* ISO C Standard: 5.2.5 An object declared as type _Bool is large
* enough to store the values 0 and 1. */
typedef int8_t _Bool;
#endif
/* ISO C Standard: 7.16 Boolean type */
#if defined(__STDC__) && defined(__GNUC__)
/* Avoid warning "ISO C90/89 does not support boolean types" */
#define bool int8_t
#else
#define bool _Bool
#endif
#define true 1
#define false 0
#define __bool_true_false_are_defined 1
#endif
#endif
#endif
src/drivers/ins/vectornav/libvnc/include/vn/const.h
+7
View File
@@ -0,0 +1,7 @@
#ifndef __VNCONTS_H__
#define __VNCONTS_H__
/** Pi in double format. */
#define VNC_PI_D (3.141592653589793238)
#endif
src/drivers/ins/vectornav/libvnc/include/vn/conv.h
+16
View File
@@ -0,0 +1,16 @@
#ifndef __VNCONV_H__
#define __VNCONV_H__
#include "vn/math/vector.h"
/** Converts ECEF coordinate to LLA frame.
* \param[in] ecef Coordinate in ECEF frame.
* \return Coordinate converted to LLA frame. */
vec3d ecef_to_lla_v3d(vec3d ecef);
/** Converts LLA coordinate to ECEF frame.
* \param[in] lla Coordinate in LLA frame in (deg, deg, meter) units.
* \return Coordinate converted to ECEF frame in (km, km, km) units. */
vec3d lla_to_ecef_v3d(vec3d lla);
#endif
src/drivers/ins/vectornav/libvnc/include/vn/enum.h
+670
View File
File diff suppressed because it is too large Load Diff
src/drivers/ins/vectornav/libvnc/include/vn/error.h
+14
View File
@@ -0,0 +1,14 @@
#ifndef VN_ERROR_H_INCLUDED
#define VN_ERROR_H_INCLUDED
#include "vn/enum.h"
/** \brief Unified error code system. */
/** \brief Converts a VnError enum into a string.
*
* \param[out] out The buffer to place the string in.
* \param[in] val The VnError value to convert to string. */
void strFromVnError(char* out, VnError val);
#endif
src/drivers/ins/vectornav/libvnc/include/vn/error_detection.h
+34
View File
@@ -0,0 +1,34 @@
#ifndef VNERRDET_H_INCLUDED
#define VNERRDET_H_INCLUDED
/** \brief Error-detection capabilities. */
#include "vn/enum.h"
#include "vn/int.h"
#include "vn/types.h"
#ifdef __cplusplus
extern "C" {
#endif
/** \brief Computes the 8-bit XOR checksum of the provided data.
*
* \param[in] data Pointer to the start of data to perform the checksum of.
* \param[in] length The number of bytes to include in the checksum.
* \return The computed checksum.
*/
uint8_t VnChecksum8_compute(char const *data, size_t length);
/** \brief Computes the 16-bit CRC16-CCITT of the provided data.
*
* \param[in] data Pointer to the start of data to perform the CRC of.
* \param[in] length The number of bytes to include in the CRC.
* \return The computed CRC.
*/
uint16_t VnCrc16_compute(char const *data, size_t length);
#ifdef __cplusplus
}
#endif
#endif
src/drivers/ins/vectornav/libvnc/include/vn/int.h
+29
View File
@@ -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 <stdint.h>
#endif
#ifdef _WIN32
#pragma warning(pop)
#endif
#endif
src/drivers/ins/vectornav/libvnc/include/vn/math/matrix.h
+87
View File
@@ -0,0 +1,87 @@
#ifndef VN_MATRIX_H_INCLUDED
#define VN_MATRIX_H_INCLUDED
#include "vn/util/compiler.h"
#ifdef __cplusplus
extern "C" {
#endif
/** \brief Represents a 3x3 matrix with an underlying data type of <c>float</c>. */
typedef union
{
float e[3*3]; /**< The matrix's elements in column-major ordering. */
/* Check if the compiler supports anonymous unions. */
#if defined(__STDC_VERSION___) && (__STDC_VERSION__ >= 201112L) && defined(__GNUC__)
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]. */
};
#endif
} mat3f;
/** \brief Represents a quaternion reading with underlying data type of <c>float</c>. */
typedef union
{
float c[4]; /**< Indexable. */
/* Check if the compiler supports anonymous unions. */
#if defined(__STDC_VERSION___) && (__STDC_VERSION__ >= 201112L) && defined(__GNUC__)
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. */
};
#endif
} 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
src/drivers/ins/vectornav/libvnc/include/vn/math/vector.h
+172
View File
@@ -0,0 +1,172 @@
#ifndef VN_VECTOR_H_INCLUDED
#define VN_VECTOR_H_INCLUDED
#include "vn/util/compiler.h"
/** \brief Various vector types and operations. */
#ifdef __cplusplus
extern "C" {
#endif
/** \brief Represents a 3 component vector with an underlying data type of
* <c>float</c>. */
typedef union
{
float c[3]; /**< Indexable. */
/* Check if the compiler supports anonymous unions. */
#if defined(__STDC_VERSION___) && (__STDC_VERSION__ >= 201112L) && defined(__GNUC__)
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. */
};
#endif
} vec3f;
/** \brief Represents a 3 component vector with an underlying data type of
* <c>double</c>. */
typedef union
{
double c[3]; /**< Indexable. */
/* Check if the compiler supports anonymous unions. */
#if defined(__STDC_VERSION___) && (__STDC_VERSION__ >= 201112L) && defined(__GNUC__)
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. */
};
#endif
} vec3d;
/** \brief Represents a 4 component vector with an underlying data type of
* <c>float</c>. */
typedef union
{
float c[4]; /**< Indexable. */
/* Check if the compiler supports anonymous unions. */
#if (defined(__STDC_VERSION___) && (__STDC_VERSION__ >= 201112L)) && defined(__GNUC__)
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. */
};
#endif
} 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
src/drivers/ins/vectornav/libvnc/include/vn/protocol/common.h
+61
View File
@@ -0,0 +1,61 @@
#ifndef _VNCOMMON_H_
#define _VNCOMMON_H_
#ifdef __cplusplus
extern "C" {
#endif
/** \brief Enumeration of the available asynchronous ASCII message types. */
typedef enum
{
VNOFF = 0, /**< Asynchronous output is turned off. */
VNYPR = 1, /**< Asynchronous output type is Yaw, Pitch, Roll. */
VNQTN = 2, /**< Asynchronous output type is Quaternion. */
#ifdef EXTRA
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 EXTRA
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 EXTRA
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 EXTRA
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. */
VNRTK = 31 /**< Asynchronous output type is RTK, from the GPS processor. */
#ifdef EXTRA
,
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
} VnAsciiAsync;
#ifdef __cplusplus
}
#endif
#endif
src/drivers/ins/vectornav/libvnc/include/vn/protocol/spi.h
+2481
View File
File diff suppressed because it is too large Load Diff
src/drivers/ins/vectornav/libvnc/include/vn/protocol/upack.h
+3650
View File
File diff suppressed because it is too large Load Diff
src/drivers/ins/vectornav/libvnc/include/vn/protocol/upackf.h
+129
View File
@@ -0,0 +1,129 @@
#ifndef VNUPACKF_H_INCLUDED
#define VNUPACKF_H_INCLUDED
#include "vn/protocol/upack.h"
/*#include "vnint.h"*/
/*#include "vnbool.h"*/
/*#include "vntypes.h"
#include "vnerror.h"*/
#ifndef VNUART_PROTOCOL_BUFFER_SIZE
/** Default internal buffers size for handling received UART data. */
#define VNUART_PROTOCOL_BUFFER_SIZE 256
#endif
#ifdef __cplusplus
extern "C" {
#endif
/** \brief Defines signature of functions that can handle callback
* notifications of packets successfully received and validated from a
* VectorNav sensor. */
typedef void(*VnUartPacketFinder_PacketFoundHandler)(void *userData, VnUartPacket* packet, size_t runningIndexOfPacketStart);
#ifdef _WIN32
#pragma warning(push)
#pragma warning(disable : 4820)
#endif
/** \brief Data structure holding current parsing status of data received from
* a VectorNav sensor.
*
* This structure contains a buffer which will hold bytes that are currently
* being processed. The size of this buffer can be adjusted by defining the
* size using the preprocesser. For example, the size can be adjusted to use
* 1024 bytes by defining VNUART_PROTOCOL_BUFFER_SIZE=1024. */
typedef struct
{
/** \brief Callback for when a packet has been found and validated. */
VnUartPacketFinder_PacketFoundHandler packetFoundHandler;
/** \brief User data for callbacks on the packetFoundHandler. */
void *packetFoundHandlerUserData;
/** \brief Used for correlating the position in the received raw data
* stream where packets are found. */
size_t runningDataIndex;
/** \brief Indicates if an ASCII packet is currently being built. */
bool asciiCurrentlyBuildingPacket;
/** \brief Indicates a suspected start of an ASCII packet. */
size_t asciiPossibleStartOfPacketIndex;
/** \brief Index of start of ASCII packet in total running index. */
size_t asciiRunningDataIndexOfStart;
/** \brief Indicates if the first ending character has been found. */
bool asciiEndChar1Found;
/** \brief Indicates if we are currently building a binary packet. */
bool binaryCurrentlyBuildingBinaryPacket;
/** \brief Index of start of binary packet in total running index. */
size_t binaryRunningDataIndexOfStart;
/** \brief Holds the size of the receive buffer. */
size_t bufferSize;
/** \brief The current location to append data into the buffer. */
size_t bufferAppendLocation;
/** \brief Indicates if we have found the groups present data field for a
* binary packet we are building. */
bool binaryGroupsPresentFound;
/** \brief The groups present found from a binary packet. */
uint8_t binaryGroupsPresent;
/** \brief Indicates the number of bytes remaining to have all group fields
* for a binary data packet we are processing. */
uint8_t binaryNumOfBytesRemainingToHaveAllGroupFields;
/** \brief Start index of a possible binary packet. */
size_t binaryPossibleStartIndex;
/** \brief Keeps track of the number of bytes remaining for a complete
* binary packet. */
size_t binaryNumberOfBytesRemainingForCompletePacket;
/** \brief The receive buffer. */
uint8_t receiveBuffer[VNUART_PROTOCOL_BUFFER_SIZE];
} VnUartPacketFinder;
#ifdef _WIN32
#pragma warning(pop)
#endif
/** \brief Initializes the VnUartPacketFinder data structure.
*
* \param[in] pf VnUartPacketFinder to init. */
void VnUartPacketFinder_initialize(VnUartPacketFinder* pf);
/** \brief Processes received data from the UART.
*
* \param[in] finder The associated VnUartPacketFinder containing the data
* processing state.
* \param[in] data The received data.
* \param[in] len The number of bytes to process.
* \param[in] bootloaderFilter Enable filtering for bootloader messages. */
VnError VnUartPacketFinder_processData_ex(VnUartPacketFinder* finder, uint8_t* data, size_t len, bool bootloaderFilter);
VnError VnUartPacketFinder_processData(VnUartPacketFinder* finder, uint8_t* data, size_t len);
/** \brief Allows registering for notification of when valid packets are found.
*
* \param[in] handler The callback function for receiving notifications.
* \param[in] userData Pointer to user supplied data which will be sent on all callback notifications.
* \return Any errors encountered. */
VnError VnUartPacketFinder_registerPacketFoundHandler(VnUartPacketFinder* finder, VnUartPacketFinder_PacketFoundHandler handler, void *userData);
#ifdef __cplusplus
}
#endif
#endif
src/drivers/ins/vectornav/libvnc/include/vn/sensors.h
+2020
View File
File diff suppressed because it is too large Load Diff
src/drivers/ins/vectornav/libvnc/include/vn/sensors/compositedata.h
+245
View File
@@ -0,0 +1,245 @@
#ifndef _VNCOMPOSITEDATA_H_
#define _VNCOMPOSITEDATA_H_
#include "vn/bool.h"
#include "vn/xplat/criticalsection.h"
#include "vn/enum.h"
#include "vn/int.h"
#include "vn/math/vector.h"
#include "vn/math/matrix.h"
#include "vn/protocol/upack.h"
#include "vn/math/vector.h"
#ifdef __cplusplus
extern "C" {
#endif
#ifdef _WIN32
#pragma warning(push)
#pragma warning(disable : 4820)
#endif
/** \brief Composite structure of all available data types from VectorNav sensors. */
typedef struct
{
vec3f yawPitchRoll; /**< Yaw, pitch, roll data. */
vec4f quaternion; /**< Quaternion data. */
mat3f directionCosineMatrix; /**< Direction cosine matrix data. */
vec3d positionGpsLla; /**< GPS latitude, longitude, altitude data. */
vec3d positionGpsEcef; /**< GPS earth-centered, earth-fixed data. */
vec3d positionEstimatedLla; /**< Estimated latitude, longitude, altitude data. */
vec3d positionEstimatedEcef; /**< Estimated earth-centered, earth-fixed position data. */
VelocityType velocityType; /**< Type of velocity in the struct. */
vec3f velocityGpsNed; /**< GPS velocity NED data. */
vec3f velocityGpsEcef; /**< GPS velocity ECEF data. */
vec3f velocityEstimatedBody; /**< Estimated velocity body data. */
vec3f velocityEstimatedNed; /**< Estimated velocity NED data. */
vec3f velocityEstimatedEcef; /**< Estimated velocity ECEF data. */
vec3f magnetic; /**< Magnetic data. */
vec3f magneticUncompensated; /**< Magnetic uncompensated data. */
vec3f magneticNed; /**< Magnetic NED data. */
vec3f magneticEcef; /**< Magnetic ECEF data. */
#ifdef EXTRA
vec3f magneticRaw; /**< Magnetic raw data. */
#endif
vec3f acceleration; /**< Acceleration data. */
vec3f accelerationUncompensated; /**< Acceleration uncompensated data. */
vec3f accelerationNed; /**< Acceleration NED data. */
vec3f accelerationEcef; /**< Acceleration ECEF data. */
vec3f accelerationLinearBody; /**< Acceleration linear body data. */
vec3f accelerationLinearNed; /**< Acceleration linear NED data. */
vec3f accelerationLinearEcef; /**< Acceleration linear ECEF data. */
#ifdef EXTRA
vec3f accelerationRaw; /**< Acceleration raw data. */
#endif
vec3f angularRate; /**< Angular rate data. */
vec3f angularRateUncompensated; /**< Angular rate uncompensated data. */
#ifdef EXTRA
vec3f angularRateRaw; /**< Angular rate raw data. */
#endif
float temperature; /**< Temperature data. */
#ifdef EXTRA
float temperatureRaw; /**< Temperature raw data. */
#endif
float pressure; /**< Pressure data. */
uint64_t timeStartup; /**< Time startup data. */
float deltaTime; /**< Delta time data. */
vec3f deltaTheta; /**< Delta theta data. */
vec3f deltaVelocity; /**< Delta velocity data. */
double tow; /**< GPS time of week data. */
uint16_t week; /**< Week data. */
uint8_t gpsFix; /**< GPS fix data. */
uint8_t numSats; /**< NumSats data. */
uint64_t timeGps; /**< TimeGps data. */
uint64_t timeGpsPps; /**< TimeGpsPps data. */
TimeUtc timeUtc; /**< TimeUtc data. */
uint64_t gpsTow; /**< GpsTow data. */
vec3f attitudeUncertainty; /**< Attitude uncertainty data. */
vec3f positionUncertaintyGpsNed; /**< GPS position uncertainty NED data. */
vec3f positionUncertaintyGpsEcef; /**< GPS position uncertainty ECEF data. */
float positionUncertaintyEstimated; /**< Estimated position uncertainty data. */
float velocityUncertaintyGps; /**< GPS velocity uncertainty data. */
float velocityUncertaintyEstimated; /**< Estimated velocity uncertainty data. */
uint32_t timeUncertainty; /**< Time uncertainty data. */
uint16_t vpeStatus; /**< VpeStatus data. */
uint16_t insStatus; /**< InsStatus data. */
uint64_t timeSyncIn; /**< TimeSyncIn data. */
uint32_t syncInCnt; /**< SyncInCnt data. */
uint32_t syncOutCnt; /**< SyncInCnt data. */
uint16_t sensSat; /**< SensSat data. */
#ifdef EXTRA
vec3f yprRates; /**< YprRates data. */
#endif
vec3d positionGps2Lla; /**< GPS2 latitude, longitude, altitude data. */
vec3d positionGps2Ecef; /**< GPS2 earth-centered, earth-fixed data. */
vec3f velocityGps2Ned; /**< GPS2 velocity NED data. */
vec3f velocityGps2Ecef; /**< GPS2 velocity ECEF data. */
uint16_t weekGps2; /**< GPS2 Week data. */
uint8_t fixGps2; /**< GPS2 fix data. */
uint8_t numSatsGps2; /**< GPS2 NumSats data. */
uint64_t timeGps2; /**< GPS2 TimeGps data. */
uint64_t timeGps2Pps; /**< GPS2 TimeGpsPps data. */
uint64_t gps2Tow; /**< GPS2 GpsTow data. */
float velocityUncertaintyGps2; /**< GPS2velocity uncertainty data. */
vec3f positionUncertaintyGps2Ned; /**< GPS2 position uncertainty NED data. */
vec3f positionUncertaintyGps2Ecef; /**< GPS2 position uncertainty ECEF data. */
uint32_t timeUncertaintyGps2; /**< GPS2 Time uncertainty data. */
TimeInfo timeInfo;
GpsDop dop;
} VnCompositeData;
#ifdef _WIN32
#pragma warning(pop)
#endif
/** \brief Indicates if course over ground has valid data
*
* \param[in] compositeData The associated VnCompositeData structure.
* \return Flag indicating if the course over ground data is available. */
bool VnCompositeData_hasCourseOverGround(VnCompositeData* compositeData);
/** \brief Computers the course over ground from any velocity data available
*
* \param[in] compositeData The associated VnCompositeData structure.
* \param[out] courseOverGroundOut The computered course over ground.
* \return Flag indicating if the calculation was successful. */
bool VnCompositeData_courseOverGround(VnCompositeData* compositeData, float* courseOverGroundOut);
/** \brief Indicates if speed over ground has valid data..
*
* \param[in] compositeData The associated VnCompositeData structure.
* \return Flag indicating if the speed over ground data is available. */
bool VnCompositeData_hasSpeedOverGround(VnCompositeData* compositeData);
/** \brief Computers the speed over ground from any velocity data available
*
* \param[in] compositeData The associated VnCompositeData structure.
* \param[out] speedOverGroundOut The computered course over ground.
* \return Flag indicating if the calculation was successful. */
bool VnCompositeData_speedOverGround(VnCompositeData* compositeData, float* speedOverGroundOut);
/** \brief
*
* \param[in]
* \param[out]
* \return
*/
void VnCompositeData_initialize(VnCompositeData* compositeData);
/** \brief
*
* \param[in]
* \param[out]
* \return
*/
void VnCompositeData_processBinaryPacket(VnCompositeData* compositeData, VnUartPacket* packet, VnCriticalSection* criticalSection);
/** \brief
*
* \param[in]
* \param[out]
* \return
*/
void VnCompositeData_processAsciiAsyncPacket(VnCompositeData* compositeData, VnUartPacket* packet, VnCriticalSection* criticalSection);
/** \brief
*
* \param[in]
* \param[out]
* \return
*/
void VnCompositeData_processBinaryPacketCommonGroup(
VnCompositeData* compositeData,
VnUartPacket* packet,
CommonGroup groupFlags);
/** \brief
*
* \param[in]
* \param[out]
* \return
*/
void VnCompositeData_processBinaryPacketTimeGroup(
VnCompositeData* compositeData,
VnUartPacket* packet,
TimeGroup groupFlags);
/** \brief
*
* \param[in]
* \param[out]
* \return
*/
void VnCompositeData_processBinaryPacketImuGroup(
VnCompositeData* compositeData,
VnUartPacket* packet,
ImuGroup groupFlags);
/** \brief
*
* \param[in]
* \param[out]
* \return
*/
void VnCompositeData_processBinaryPacketGpsGroup(
VnCompositeData* compositeData,
VnUartPacket* packet,
GpsGroup groupFlags);
/** \brief
*
* \param[in]
* \param[out]
* \return
*/
void VnCompositeData_processBinaryPacketAttitudeGroup(
VnCompositeData* compositeData,
VnUartPacket* packet,
AttitudeGroup groupFlags);
/** \brief
*
* \param[in]
* \param[out]
* \return
*/
void VnCompositeData_processBinaryPacketInsGroup(
VnCompositeData* compositeData,
VnUartPacket* packet,
InsGroup groupFlags);
/** \brief
*
* \param[in]
* \param[out]
* \return
*/
void VnCompositeData_processBinaryPacketGps2Group(
VnCompositeData* compositeData,
VnUartPacket* packet,
GpsGroup groupFlags);
#ifdef __cplusplus
}
#endif
#endif
src/drivers/ins/vectornav/libvnc/include/vn/sensors/ezasyncdata.h
+60
View File
@@ -0,0 +1,60 @@
#ifndef _VNEZASYNCDATA_H_
#define _VNEZASYNCDATA_H_
#include "vn/int.h"
#include "vn/error.h"
#include "vn/sensors/compositedata.h"
#include "vn/sensors.h"
#ifdef __cplusplus
extern "C" {
#endif
/** \brief Structure supporting easy and reliable access to asynchronous data
* from a VectorNav sensor at the cost of a slight performance hit. */
typedef struct
{
/** \brief The associated connected sensor. */
VnSensor* sensor;
/** \brief Critical section for accessing the current data. */
VnCriticalSection* curDataCS;
/** \brief The current data received from asynchronous data packets. */
VnCompositeData* curData;
} VnEzAsyncData;
/** \brief Initializes and connects to a VectorNav sensor with the specified
* connection parameters.
*
* \param[in] ezAsyncData The associated VnEzAsyncData structure.
* \param]in] portName The name of the serial port to connect to.
* \param[in] baudrate The baudrate to connect at.
* \return Any errors encountered. */
VnError VnEzAsyncData_initializeAndConnect(VnEzAsyncData* ezAsyncData, const char* portName, uint32_t baudrate);
/** \brief Disconnects from a VectorNav sensor.
*
* \param[in] ezAsyncData The associated VnEzAsyncData structure.
* \return Any errors encountered. */
VnError VnEzAsyncData_disconnectAndUninitialize(VnEzAsyncData* ezAsyncData);
/** \brief Returns the most recent asynchronous data the VnEzAsyncData structure
* has processed.
*
* \param[in] ezAsyncData The associated VnEzAsyncData structure.
* \return The latest data processed. */
VnCompositeData VnEzAsyncData_currentData(VnEzAsyncData* ezAsyncData);
/** \brief Returns the underlying VnSensor referenced.
*
* \param[in] ezAsyncData The associated VnEzAsyncData structure.
* \return The underlying VnSensor reference. */
VnSensor* VnEzAsyncData_sensor(VnEzAsyncData* ezAsyncData);
#ifdef __cplusplus
}
#endif
#endif
src/drivers/ins/vectornav/libvnc/include/vn/sensors/searcher.h
+94
View File
@@ -0,0 +1,94 @@
#ifndef _SEARCHER_H_
#define _SEARCHER_H_
#include "vn/bool.h"
#include "vn/int.h"
#include "vn/xplat/serialport.h"
#include "vn/xplat/thread.h"
/* These defines are used to enable a single function name while implementing */
/* different solutions given the OS. */
#if defined __linux__ || defined __CYGWIN__ || defined __NUTTX__
#define VnSearcher_findPorts VnSearcher_findPorts_LINUX
#elif defined _WIN32
#define VnSearcher_findPorts VnSearcher_findPorts_WIN32
#else
#error ERROR: System not yet supported in VnSearcher
#endif
#ifdef _WIN32
#pragma warning(push)
#pragma warning(disable : 4820)
#endif
/** \brief containing information about the port to be searched. */
typedef struct
{
/** \brief Contains the name of the port. */
char* portName;
/** \brief VnThread object for callback purposes. */
VnThread thread;
/** \brief Baud of the attached senosr, -1 for no sensor, -2 for error. */
int32_t baud;
/** \brief size of the data array */
size_t dataSize;
/** \brief VnSerialPort object to handle communication to the sensor. */
VnSerialPort* port;
/** \brief Array to store partial/completed communication data from the sensor. */
char data[255];
} VnPortInfo;
#ifdef _WIN32
#pragma warning(pop)
#endif
/** \brief Takes a VnPortInfo and returns the name of the port.
* \parm[in] portInfo Pointer to a VnPortInfo struct
* \return The name of the port
*/
char* VnSearcher_getPortName(VnPortInfo* portInfo);
/** \brief Takes a VnPortInfo and returns the baud rate or error code.
* \parm[in] portInfo Pointer to a VnPortInfo struct
* \return The baud rate of the port or an error code.
* -1 indicates no sensor on the port.
* -2 indicates an error trying to find a sensor.
*/
int32_t VnSearcher_getPortBaud(VnPortInfo* portInfo);
/** \brief Function to find the names of all of the system's serial ports.
* \parm[out] portNames Array containing the names of all of the serial ports.
* \parm[out] numPortsFound Number of serial ports found.
*/
void VnSearcher_findPorts(char*** portNames, int32_t* numPortsFound);
/** \brief Function to search an input port and return either the baud rate of the connected
* sensor or an error code.
* \parm[in] portName The system name of the port to be searched.
* \parm[out] foundBaudrate Baud rate of the attacked sensor, -1 for no sensor, -2 for error
*/
void VnSearcher_findPortBaud(char* portName, int32_t* foundBaudrate);
/** \brief Convenience function to find all available sensors currently attached
* to the system.
* \parm[out] sensorsFound Pointer to an array of sensors attached to the system.
* \parm[out] numSensors The number of sensors found.
*/
void VnSearcher_findAllSensors(VnPortInfo*** sensorsFound, int32_t* numSensors);
/** \brief Function that will search all of the input ports to see if an available sensor is attached to it.
* NOTE: The OS will not allow the detection of sensor that is in use.
* \parm[in] portsToCheck Array containing the names of all the ports to check.
* \parm[in] Number of ports to check.
* \parm[out] An array of VnPortInfo structs the same size as numPortsToCheck. The baud rate will indicate
* if a sensor is attached. baud > 0 is the sensor baudrate, baud = -1 no sensor attached, and baud = -2
* an error occured while detecting the sensor.
*/
void VnSearcher_searchInputPorts(char*** portsToCheck, int32_t numPortsToCheck, VnPortInfo*** sensorsFound);
#endif
src/drivers/ins/vectornav/libvnc/include/vn/types.h
+60
View File
@@ -0,0 +1,60 @@
#ifndef VN_TYPES_H_INCLUDED
#define VN_TYPES_H_INCLUDED
/** \brief Standard types used through out the library. */
#include "vn/int.h"
#if !defined(__cplusplus)
#if (defined(__STDC_VERSION__) && __STDC_VERSION__ >= 199901L) || defined(__GNUC__)
#include <stddef.h>
#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
src/drivers/ins/vectornav/libvnc/include/vn/util.h
+273
View File
@@ -0,0 +1,273 @@
#ifndef VNUTIL_H_INCLUDED
#define VNUTIL_H_INCLUDED
#include <stddef.h>
#include "vn/int.h"
#include "vn/bool.h"
#include "vn/error.h"
#include "vn/math/matrix.h"
#include "vn/math/vector.h"
#include "vn/util/export.h"
#include "vn/types.h"
#ifdef __cplusplus
extern "C" {
#endif
/** \brief Defines for the specific version of the VectorNav library. */
#define VNAPI_MAJOR 1
#define VNAPI_MINOR 2
#define VNAPI_PATCH 0
#define VNAPI_REVISION 126
/** \brief Returns the major version of the VectorNav library.
*
* \return The major version. */
int VnApi_major(void);
/** \brief Returns the minor version of the VectorNav library.
*
* \return The minor version. */
int VnApi_minor(void);
/** \brief Returns the patch version of the VectorNav library.
*
* \return The patch version. */
int VnApi_patch(void);
/** \brief Returns the revision version of the VectorNav library.
*
* \return The revision version. */
int VnApi_revision(void);
/** \brief Returns the full version of the VectorNav library as a string.
*
* \param[out] out The buffer to place the string.
* \param[in] outLength The number of characters available in the out buffer.
* \return Any errors encountered. */
VnError VnApi_getVersion(char *out, size_t outLength);
/** \brief Converts the single hexadecimal character to a uint8_t.
*
* \param[in] c The hexadecimal character to convert.
* \return The converted value.
*/
uint8_t VnUtil_toUint8FromHexChar(char c);
/** \brief Converts a 2 character hexadecimal string to a uint8_t.
*
* \param[in] str Pointer to 2 characters representing a hexadecimal
* number.
* \return The converted value.
*/
uint8_t VnUtil_toUint8FromHexStr(char const *str);
/** \brief Converts a 4 character hexadecimal string to a uint16_t.
*
* \param[in] str Pointer to 4 characters representing a hexadecimal
* number.
* \return The converted value.
*/
uint16_t VnUtil_toUint16FromHexStr(char const *str);
/** \brief Converts a uint8_t to a hexadecimal string.
*
* \param[in] toConvert The uint8_t to convert.
* \param[in] output Pointer to the char array to write the converted value.
* This will take 2 bytes for the output.
*/
void VnUtil_toHexStrFromUint8(uint8_t toConvert, char *output);
/** \brief Converts a uint16_t to a hexadecimal string.
*
* \param[in] toConvert The uint16_t to convert.
* \param[in] output Pointer to the char array to write the converted value.
* This will take 4 bytes for the output.
*/
void VnUtil_toHexStrFromUint16(uint16_t toConvert, char *output);
/** \brief Converts a uint16_t to a string.
*
* \param[in] toConvert The uint16_t to convert.
* \param[in] output Pointer to the char array to write the converted value.
* \return The number of characters that were written.
*/
size_t VnUtil_toStrFromUint16(uint16_t toConvert, char *output);
/** \brief Returns the number of bits set.
*
* \param[in] data The data value to count the number of bits set.
* \return The number of bits set.
*/
uint8_t DllExport VnUtil_countSetBitsUint8(uint8_t data);
/** \brief Converts a boolean value into a string.
*
* \param[out] out The value converted to a string.
* \param[in] val The value to convert.
*/
void strFromBool(char *out, bool val);
/** \defgroup byteOrderers Byte Ordering Functions
* \brief This group of functions are useful for ordering of bytes
* sent/received from VectorNav sensors.
*
* \{ */
/** \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 stoh16(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 stoh32(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 stoh64(uint64_t sensorOrdered);
/** \brief Converts a 16-bit integer in host order to sensor order.
*
* \param[in] hostOrdered The 16-bit integer in host order.
* \return The value converted to sensor ordered. */
uint16_t htos16(uint16_t hostOrdered);
/** \brief Converts a 32-bit integer in host order to sensor order.
*
* \param[in] hostOrdered The 32-bit integer in host order.
* \return The value converted to sensor ordered. */
uint32_t htos32(uint32_t hostOrdered);
/** \brief Converts a 64-bit integer in host order to sensor order.
*
* \param[in] hostOrdered The 64-bit integer in host order.
* \return The value converted to sensor ordered. */
uint64_t htos64(uint64_t hostOrdered);
/** \brief Converts a 4-byte float in host order to sensor order.
*
* \param[in] hostOrdered The 4-byte float in host order.
* \return The value converted to sensor ordered. */
float htosf4(float hostOrdered);
/** \brief Converts an 8-byte float in host order to sensor order.
*
* \param[in] hostOrdered The 8-byte float in host order.
* \return The value converted to sensor ordered. */
double htosf8(double hostOrdered);
/** \} */
/** \defgroup sensorValueExtractors Sensor Value Extractors
* \brief This group of methods is useful for extracting data from binary
* data received from a VectorNav sensor either from a UART binary or a
* SPI packet. Any necessary byte ordering will be performed.
*
* \{ */
/** \brief Extracts a uint16_t with appropriate byte reordering from the binary
* array received from a VectorNav sensor either from the UART binary or
* SPI packet.
*
* \param[in] pos The current position to extract the value from.
* \return The extracted value. */
uint16_t VnUtil_extractUint16(const char* pos);
/** \brief Extracts a uint32_t with appropriate byte reordering from the binary
* array received from a VectorNav sensor either from the UART binary or
* SPI packet.
*
* \param[in] pos The current position to extract the value from.
* \return The extracted value. */
uint32_t VnUtil_extractUint32(const char* pos);
/** \brief Extracts a <c>float</c> with appropriate byte reordering from the binary
* array received from a VectorNav sensor either from the UART binary or
* SPI packet.
*
* \param[in] pos The current position to extract the value from.
* \return The extracted value. */
float VnUtil_extractFloat(const char* pos);
/** \brief Extracts a <c>double</c> with appropriate byte reordering from the binary
* array received from a VectorNav sensor either from the UART binary or
* SPI packet.
*
* \param[in] pos The current position to extract the value from.
* \return The extracted value. */
double VnUtil_extractDouble(const char* pos);
/*#if THIS_SHOULD_BE_MOVED_TO_MATH_C_PACK*/
/** \brief Extracts a <c>vec3f</c> with appropriate byte reordering from the binary
* array received from a VectorNav sensor either from the UART binary or
* SPI packet.
*
* \param[in] pos The current position to extract the value from.
* \return The extracted value. */
vec3f VnUtil_extractVec3f(const char* pos);
/** \brief Extracts a <c>vec4f</c> with appropriate byte reordering from the binary
* array received from a VectorNav sensor either from the UART binary or
* SPI packet.
*
* \param[in] pos The current position to extract the value from.
* \return The extracted value. */
vec4f VnUtil_extractVec4f(const char* pos);
/** \brief Extracts a <c>vec3d</c> with appropriate byte reordering from the binary
* array received from a VectorNav sensor either from the UART binary or
* SPI packet.
*
* \param[in] pos The current position to extract the value from.
* \return The extracted value. */
vec3d VnUtil_extractVec3d(const char* pos);
/** \brief Extracts a <c>mat3f</c> with appropriate byte reordering from the binary
* array received from a VectorNav sensor either from the UART binary or
* SPI packet.
*
* \param[in] pos The current position to extract the value from.
* \return The extracted value. */
mat3f VnUtil_extractMat3f(const char* pos);
/** \brief Extracts a <c>GpsDop</c> with appropriate byte reordering from the binary
* array received from a VectorNav sensor either from the UART binary or
* SPI packet.
*
* \param[in] pos The current position to extract the value from.
* \return The extracted value. */
GpsDop VnUtil_extractGpsDop(const char* pos);
/** \brief Extracts a <c>TimeUtc</c> with appropriate byte reordering from the binary
* array received from a VectorNav sensor either from the UART binary or
* SPI packet.
*
* \param[in] pos The current position to extract the value from.
* \return The extracted value. */
TimeUtc VnUtil_extractTimeUtc(const char* pos);
/** \brief Extracts a <c>TimeInfo</c> with appropriate byte reordering from the binary
* array received from a VectorNav sensor either from the UART binary or
* SPI packet.
*
* \param[in] pos The current position to extract the value from.
* \return The extracted value. */
TimeInfo VnUtil_extractTimeInfo(const char* pos);
/*#endif*/
/* \} */
#ifdef __cplusplus
}
#endif
#endif
src/drivers/ins/vectornav/libvnc/include/vn/util/compiler.h
+37
View File
@@ -0,0 +1,37 @@
#ifndef _VN_UTIL_COMPILER_H
#define _VN_UTIL_COMPILER_H
/* This header provides some simple checks for various features supported by the
* current compiler. */
/* Determine the level of standard C support. */
#if __STDC__
#if defined (__STDC_VERSION__)
#if (__STDC_VERSION__ >= 199901L)
#define C99
#endif
#endif
#endif
/* Determine if the compiler has stdbool.h. */
#if defined(C99) || _MSC_VER >= 1800
#define VN_HAVE_STDBOOL_H 1
#else
#define VN_HAVE_STDBOOL_H 0
#endif
/* Determine if the secure CRT is available. */
#if defined(_MSC_VER)
#define VN_HAVE_SECURE_CRT 1
#else
#define VN_HAVE_SECURE_CRT 0
#endif
#endif
/* Determine if the generic type math library (tgmath.h) is available. */
#if defined(C99)
#define VN_HAVE_GENERIC_TYPE_MATH 1
#else
#define VN_HAVE_GENERIC_TYPE_MATH 0
#endif
src/drivers/ins/vectornav/libvnc/include/vn/util/export.h
+16
View File
@@ -0,0 +1,16 @@
#ifndef VNEXPORT_H_INCLUDED
#define VNEXPORT_H_INCLUDED
/* Not only does this have to be windows to use __declspec */
/* it also needs to actually be outputting a DLL */
#if defined _WINDOWS && defined _WINDLL
#if proglib_c_EXPORTS
#define DllExport __declspec(dllexport)
#else
#define DllExport __declspec(dllimport)
#endif
#else
#define DllExport
#endif
#endif
src/drivers/ins/vectornav/libvnc/include/vn/util/port.h
+19
View File
@@ -0,0 +1,19 @@
#ifndef VNPORT_H_INCLUDED
#define VNPORT_H_INCLUDED
/** Basic portability measures. */
/** VNAPI - DLL linkage specifier. */
#ifdef _MSC_VER
#if VN_LINKED_AS_SHARED_LIBRARY
#define VNAPI __declspec(dllimport)
#elif VN_CREATE_SHARED_LIBRARY
#define VNAPI __declspec(dllexport)
#endif
#endif
#ifndef VNAPI
#define VNAPI
#endif
#endif
src/drivers/ins/vectornav/libvnc/include/vn/vectornav.h
+9
View File
@@ -0,0 +1,9 @@
#ifndef VECTORNAV_H
#define VECTORNAV_H
#include "vnbool.h"
#include "vnenum.h"
#include "vnutil.h"
#include "vnupackf.h"
#endif
src/drivers/ins/vectornav/libvnc/include/vn/xplat/criticalsection.h
+77
View File
@@ -0,0 +1,77 @@
/** \file
* {COMMON_HEADER}
*
* \section DESCRIPTION
* This header file contains structures and functions useful for critical sections.
*/
#ifndef _VN_CRITICALSECTION_H_
#define _VN_CRITICALSECTION_H_
#include "vn/error.h"
#ifdef _WIN32
/* Disable some warnings for Visual Studio with -Wall. */
#if defined(_MSC_VER)
#pragma warning(push)
#pragma warning(disable:4668)
#pragma warning(disable:4820)
#pragma warning(disable:4255)
#endif
#include <Windows.h>
#if defined(_MSC_VER)
#pragma warning(pop)
#endif
#endif
#if (defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__)
#include <pthread.h>
#endif
#ifdef __cplusplus
extern "C" {
#endif
typedef struct
{
#if _WIN32
CRITICAL_SECTION handle;
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
pthread_mutex_t handle;
#else
#error "Unknown System"
#endif
} VnCriticalSection;
/** \breif Initializes a VnCriticalSection structure.
*
* \param[in] criticalSection The VnCriticalSection structure to initialize.
* \return Any errors encountered. */
VnError VnCriticalSection_initialize(VnCriticalSection *criticalSection);
/** \brief Disposes of a VnCriticalSection structure and associated resources.
*
* \param[in] criticalSection The associated VnCriticalSection structure.
* \return Any errors encountered. */
VnError VnCriticalSection_deinitialize(VnCriticalSection *criticalSection);
/** \brief Attempt to enter a critical section.
*
* \param[in] criticalSection The associated VnCriticalSection structure.
* \return Any errors encountered. */
VnError VnCriticalSection_enter(VnCriticalSection *criticalSection);
/** \brief Leave a critical section.
*
* \param[in] criticalSection The associated VnCriticalSection structure.
* \return Any errors encountered. */
VnError VnCriticalSection_leave(VnCriticalSection *criticalSection);
#ifdef __cplusplus
}
#endif
#endif
src/drivers/ins/vectornav/libvnc/include/vn/xplat/event.h
+98
View File
@@ -0,0 +1,98 @@
/** \file
* {COMMON_HEADER}
*
* \section DESCRIPTION
* This header file contains structures and functions useful events and signals.
*/
#ifndef _VNEVENT_H_
#define _VNEVENT_H_
#include "vn/int.h"
#include "vn/error.h"
#include "vn/bool.h"
#ifdef _WIN32
/* Disable some warnings for Visual Studio with -Wall. */
#if defined(_MSC_VER)
#pragma warning(push)
#pragma warning(disable:4668)
#pragma warning(disable:4820)
#pragma warning(disable:4255)
#endif
#include <Windows.h>
#if defined(_MSC_VER)
#pragma warning(pop)
#endif
#endif
#if (defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__)
#include <pthread.h>
#endif
#ifdef __cplusplus
extern "C" {
#endif
/** \brief Structure representing an event. */
typedef struct
{
#ifdef _WIN32
HANDLE handle;
#elif (defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__)
pthread_mutex_t mutex;
pthread_cond_t condition;
bool isTriggered;
#else
#error "Unknown System"
#endif
} VnEvent;
/** \brief Initializes a VnEvent structure.
*
* \param[in] event The VnEvent structure to initialize.
* \return Any errors encountered. */
VnError VnEvent_initialize(VnEvent *event);
/** \brief Causes the calling thread to wait on an event until the event is signalled.
*
* If the event is signalled, the value E_SIGNALED will be returned.
*
* \param[in] event The associated VnEvent.
* \return Any errors encountered. */
VnError VnEvent_wait(VnEvent *event);
/** \brief Causes the calling thread to wait on an event until the event is signalled.
*
* If the event is signalled, the value E_SIGNALED will be returned.
*
* \param[in] event The associated VnEvent.
* \param[in] timeoutUs The number of microseconds to wait before the thread stops waiting on the event. If a timeout
* does occur, the value E_TIMEOUT will be returned.
* \return Any errors encountered. */
VnError VnEvent_waitUs(VnEvent *event, uint32_t timeoutUs);
/** \brief Causes the calling thread to wait on an event until the event is signalled.
*
* If the event is signalled, the value E_SIGNALED will be returned.
*
* \param[in] event The associated VnEvent.
* \param[in] timeoutMs The number of milliseconds to wait before the thread stops waiting on the event. If a timeout
* does occur, the value E_TIMEOUT will be returned.
* \return Any errors encountered. */
VnError VnEvent_waitMs(VnEvent *event, uint32_t timeoutMs);
/** \brief Signals an event.
*
* \param[in] event The associated event.
* \return Any errors encountered. */
VnError VnEvent_signal(VnEvent *event);
#ifdef __cplusplus
}
#endif
#endif
src/drivers/ins/vectornav/libvnc/include/vn/xplat/serialport.h
+149
View File
@@ -0,0 +1,149 @@
#ifndef VN_SERIALPORT_H
#define VN_SERIALPORT_H
/** Cross-platform access to serial ports. */
#include "vn/int.h"
#include "vn/error.h"
#include "vn/bool.h"
#include "vn/xplat/thread.h"
#include "vn/xplat/criticalsection.h"
#if defined(_WIN32)
/* Disable some warnings for Visual Studio with -Wall. */
#if defined(_MSC_VER)
#pragma warning(push)
#pragma warning(disable:4668)
#pragma warning(disable:4820)
#pragma warning(disable:4255)
#endif
#include <Windows.h>
#if defined(_MSC_VER)
#pragma warning(pop)
#endif
#endif
#ifdef __linux__
#include <linux/serial.h>
#elif defined __APPLE__
#include <dirent.h>
#endif
#ifdef __cplusplus
extern "C" {
#endif
/** \brief Type for listening to data received events from the VnSerialPort. */
typedef void (*VnSerialPort_DataReceivedHandler)(void *userData);
#ifdef _WIN32
#pragma warning(push)
#pragma warning(disable : 4820)
#endif
/** \brief Provides access to a serial port. */
typedef struct
{
#ifdef _WIN32
HANDLE handle;
/* Windows appears to need single-thread access to read/write API functions. */
VnCriticalSection readWriteCS;
#elif (defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__)
int handle;
#else
#error "Unknown System"
#endif
bool purgeFirstDataBytesWhenSerialPortIsFirstOpened;
VnSerialPort_DataReceivedHandler dataReceivedHandler;
void *dataReceivedHandlerUserData;
VnThread serialPortNotificationsThread;
bool continueHandlingSerialPortEvents;
size_t numberOfReceiveDataDroppedSections;
VnCriticalSection dataReceivedHandlerCriticalSection;
char portName[50];
} VnSerialPort;
#ifdef _WIN32
#pragma warning(pop)
#endif
/** \brief Initializes a VnSerialPort structure.
*
* \param[in] serialport The VnSerialPort structure to initialize.
* \return Any errors encountered. */
VnError VnSerialPort_initialize(VnSerialPort *serialport);
/** \brief Opens a serial port.
*
* \param[in] serialport The associated VnSerialPort structure.
* \param[in] portName The name of the serial port to open.
* \param[in] baudrate The baudrate to open the serial port at.
* \return Any errors encountered. */
VnError VnSerialPort_open(VnSerialPort *serialport, char const *portName, uint32_t baudrate);
/** \brief Closes the serial port.
*
* \param[in] serialport The associated VnSerialPort structure.
* \return Any errors encountered. */
VnError VnSerialPort_close(VnSerialPort *serialport);
/** \brief Indicates if the serial port is open or not.
*
* \param[in] serialport The associated VnSerialPort structure.
* \return <c>true</c> if the serial port is open; otherwise <c>false</c>. */
bool VnSerialPort_isOpen(VnSerialPort *serialport);
/** \brief Reads data from a serial port.
*
* \param[in] serialport The associated VnSerialPort structure.
* \param[in] buffer Buffer to place the read bytes.
* \param[in] numOfBytesToRead The number of bytes to read from the serial port.
* \param[out] numOfBytesActuallyRead The number of bytes actually read from the serial port.
* \return Any errors encountered. */
VnError VnSerialPort_read(VnSerialPort *serialport, char *buffer, size_t numOfBytesToRead, size_t *numOfBytesActuallyRead);
/** \brief Writes data out of a serial port.
*
* \param[in] serialport The associated VnSerialPort.
* \param[in] data The data to write out.
* \param[in] numOfBytesToWrite The number of bytes to write out of the serial port.
* \return Any errors encountered. */
VnError VnSerialPort_write(VnSerialPort *serialport, char const *data, size_t numOfBytesToWrite);
/** \brief Changes the baudrate the port is connected at.
*
* \param[in] serialport The associated VnSerialPort.
* \param[in] baudrate The new baudrate.
* \return Any errors encountered. */
VnError VnSerialPort_changeBaudrate(VnSerialPort *serialport, uint32_t baudrate);
/** \brief Allows registering for notification of data received events.
*
* \param[in] serialPort The associated VnSerialPort.
* \param[in] handler The callback method to receive notifications.
* \param[in] userData User supplied data that will be sent to the handler on callbacks.
* \return Any errors encountered. */
VnError VnSerialPort_registerDataReceivedHandler(VnSerialPort *serialPort, VnSerialPort_DataReceivedHandler handler, void *userData);
/** \brief Allows unregistering for notification of data received events.
*
* \param[in] serialPort The associated VnSerialPort. */
VnError VnSerialPort_unregisterDataReceivedHandler(VnSerialPort *serialPort);
#ifdef __cplusplus
}
#endif
#endif
src/drivers/ins/vectornav/libvnc/include/vn/xplat/thread.h
+87
View File
@@ -0,0 +1,87 @@
/** \file
* {COMMON_HEADER}
*
* \section DESCRIPTION
* This header file contains structures and functions useful working with threads.
*/
#ifndef _VN_THREAD_H_
#define _VN_THREAD_H_
#include "vn/error.h"
#include "vn/int.h"
#ifdef _WIN32
/* Disable some warnings for Visual Studio with -Wall. */
#if defined(_MSC_VER)
#pragma warning(push)
#pragma warning(disable:4668)
#pragma warning(disable:4820)
#pragma warning(disable:4255)
#endif
#include <Windows.h>
#if defined(_MSC_VER)
#pragma warning(pop)
#endif
#endif
#if defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__
#include <pthread.h>
#endif
#ifdef __cplusplus
extern "C" {
#endif
/** \brief Structure for working with threads. */
typedef struct
{
#ifdef _WIN32
HANDLE handle;
#elif defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__
pthread_t handle;
#else
#error "Unknown System"
#endif
} VnThread;
/** \brief Function signature for a start routine for a thread. */
typedef void (*VnThread_StartRoutine)(void*);
/** \brief Starts a new thread immediately which calls the provided start routine.
*
* \param[in] thread Associated VnThread structure.
* \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 routine on the new thread.
* \return Any errors encountered. */
VnError VnThread_startNew(VnThread *thread, VnThread_StartRoutine startRoutine, void* routineData);
/** \brief Blocks the calling thread until the referenced thread finishes.
*
* \param[in] thread The associated VnThread.
* \return Any errors encountered. */
VnError VnThread_join(VnThread *thread);
/** \brief Causes the calling thread to sleep the specified number of seconds.
*
* \param[in] numOfSecsToSleep The number of seconds to sleep. */
void VnThread_sleepSec(uint32_t numOfSecsToSleep);
/** \brief Causes the calling thread to sleep the specified number of milliseconds
*
* \param[in] numOfMsToSleep The number of milliseconds to sleep. */
void VnThread_sleepMs(uint32_t numOfMsToSleep);
/** \brief Causes the calling thread to sleep the specified number of microseconds
*
* \param[in] numOfUsToSleep The number of microseconds to sleep. */
void VnThread_sleepUs(uint32_t numOfUsToSleep);
#ifdef __cplusplus
}
#endif
#endif
src/drivers/ins/vectornav/libvnc/include/vn/xplat/time.h
+73
View File
@@ -0,0 +1,73 @@
/** \file
* {COMMON_HEADER}
*
* \section DESCRIPTION
* This header file contains structures and functions useful timing.
*/
#ifndef _VNTIME_H_
#define _VNTIME_H_
#include "vn/int.h"
#include "vn/error.h"
#include "vn/enum.h"
#ifdef _WIN32
/* Disable some warnings for Visual Studio with -Wall. */
#if defined(_MSC_VER)
#pragma warning(push)
#pragma warning(disable:4668)
#pragma warning(disable:4820)
#pragma warning(disable:4255)
#endif
#include <Windows.h>
#if defined(_MSC_VER)
#pragma warning(pop)
#endif
#endif
#ifdef __cplusplus
extern "C" {
#endif
/** \brief Provides simple timing capabilities. */
typedef struct
{
#if _WIN32
double pcFrequency;
__int64 counterStart;
#elif __linux__ || __APPLE__ ||__CYGWIN__ || __QNXNTO__ || defined __NUTTX__
double clockStart;
#else
#error "Unknown System"
#endif
} VnStopwatch;
/** \brief Initializes and starts a stopwatch.
*
* \param[in] stopwatch The VnStopwatch to initialize and start.
* \return Any errors encountered. */
VnError VnStopwatch_initializeAndStart(VnStopwatch *stopwatch);
/** \brief Resets the stopwatch's timing.
*
* \param[in] stopwatch The associated VnStopwatch.
* \return Any errors encountered. */
VnError VnStopwatch_reset(VnStopwatch *stopwatch);
/** \brief Determines the number of milliseconds elapsed since the last reset
* of the stopwatch.
*
* \param[in] stopwatch The associated VnStopwatch.
* \param[out] elapsedMs The elapsed time in milliseconds.
* \return Any errors encountered. */
VnError VnStopwatch_elapsedMs(VnStopwatch *stopwatch, float *elapsedMs);
#ifdef __cplusplus
}
#endif
#endif
src/drivers/ins/vectornav/libvnc/src/vn/conv.c
+81
View File
@@ -0,0 +1,81 @@
#include "vn/conv.h"
#include <math.h>
#include "vn/const.h"
#define C_E2 (0.006694379990141)
#define C_EPSILON (0.996647189335253)
#define C_ABAR (42.69767270717997)
#define C_BBAR (42.84131151331357)
#define C_A (6378.137)
vec3d ecef_to_lla_v3d(vec3d ecef)
{
double x, y, z, r, c_phi, c_phi0, s_phi,
s_phi0, tau, lat, lon, /*alt,*/ eta, h;
const double Rthresh = 0.001; /* Limit on distance from pole in km to switch calculation. */
x = ecef.c[0];
y = ecef.c[1];
z = ecef.c[2];
r = sqrt(x*x + y*y);
if (r < Rthresh)
{
c_phi0 = 0;
s_phi0 = (z > 0) - (z < 0); /* computes sign */
}
else
{
double tau0 = z / (C_EPSILON * r);
c_phi0 = 1 / sqrt(1 + tau0 * tau0);
s_phi0 = tau0 * c_phi0;
}
tau = (z + C_BBAR * s_phi0 * s_phi0 * s_phi0) / (r - C_ABAR * c_phi0 * c_phi0 * c_phi0);
lat = atan(tau);
if (r < Rthresh)
{
c_phi = 0;
s_phi = (z > 0) - (z < 0); /* computes sign */
}
else
{
c_phi = 1 / sqrt(1 + tau * tau);
s_phi = tau * c_phi;
}
eta = sqrt(1 - C_E2 * s_phi * s_phi);
h = r * c_phi + z * s_phi - C_A * eta;
lon = atan2(y, x);
return create_v3d(
lat * 180 / VNC_PI_D,
lon * 180 / VNC_PI_D,
h * 1000
);
}
vec3d lla_to_ecef_v3d(vec3d lla)
{
double lat, lon, alt;
double n, x, y, z;
double t1; /* TEMPS */
lat = lla.c[0] * VNC_PI_D / 180; /* Geodetic latitude in radians. */
lon = lla.c[1] * VNC_PI_D / 180; /* Longitude in radians. */
alt = lla.c[2] / 1000; /* Altitude above WGS84 in km. */
t1 = sin(lat);
n = C_A / sqrt(1 - C_E2 * t1 * t1);
t1 = alt + n;
x = t1 * cos(lat) * cos(lon);
y = t1 * cos(lat) * sin(lon);
z = (t1 - C_E2 * n) * sin(lat);
return create_v3d(x, y, z);
}
src/drivers/ins/vectornav/libvnc/src/vn/error.c
+142
View File
@@ -0,0 +1,142 @@
#include "vn/error.h"
#include <string.h>
void strFromVnError(char* out, VnError val)
{
#if defined(_MSC_VER)
/* Disable warnings regarding using strcpy_s since this
* function's signature does not provide us with information
* about the length of 'out'. */
#pragma warning(push)
#pragma warning(disable:4996)
#endif
switch (val)
{
case E_NONE:
strcpy(out, "NONE");
break;
case E_UNKNOWN:
strcpy(out, "UNKNOWN");
break;
case E_BUFFER_TOO_SMALL:
strcpy(out, "BUFFER_TOO_SMALL");
break;
case E_INVALID_VALUE:
strcpy(out, "INVALID_VALUE");
break;
case E_NOT_IMPLEMENTED:
strcpy(out, "NOT_IMPLEMENTED");
break;
case E_NOT_SUPPORTED:
strcpy(out, "NOT_SUPPORTED");
break;
case E_NOT_FOUND:
strcpy(out, "NOT_FOUND");
break;
case E_TIMEOUT:
strcpy(out, "TIMEOUT");
break;
case E_PERMISSION_DENIED:
strcpy(out, "PERMISSION_DENIED");
break;
case E_INVALID_OPERATION:
strcpy(out, "INVALID_OPERATION");
break;
case E_SIGNALED:
strcpy(out, "SIGNALED");
break;
case E_SENSOR_HARD_FAULT:
strcpy(out, "SENSOR_HARD_FAULT");
break;
case E_SENSOR_SERIAL_BUFFER_OVERFLOW:
strcpy(out, "SENSOR_SERIAL_BUFFER_OVERFLOW");
break;
case E_SENSOR_INVALID_CHECKSUM:
strcpy(out, "SENSOR_INVALID_CHECKSUM");
break;
case E_SENSOR_INVALID_COMMAND:
strcpy(out, "SENSOR_INVALID_COMMAND");
break;
case E_SENSOR_NOT_ENOUGH_PARAMETERS:
strcpy(out, "SENSOR_NOT_ENOUGH_PARAMETERS");
break;
case E_SENSOR_TOO_MANY_PARAMETERS:
strcpy(out, "SENSOR_TOO_MANY_PARAMETERS");
break;
case E_SENSOR_INVALID_PARAMETER:
strcpy(out, "SENSOR_INVALID_PARAMETER");
break;
case E_SENSOR_INVALID_REGISTER:
strcpy(out, "SENSOR_INVALID_REGISTER");
break;
case E_SENSOR_UNAUTHORIZED_ACCESS:
strcpy(out, "SENSOR_UNAUTHORIZED_ACCESS");
break;
case E_SENSOR_WATCHDOG_RESET:
strcpy(out, "SENSOR_WATCHDOG_RESET");
break;
case E_SENSOR_OUTPUT_BUFFER_OVERFLOW:
strcpy(out, "SENSOR_OUTPUT_BUFFER_OVERFLOW");
break;
case E_SENSOR_INSUFFICIENT_BAUD_RATE:
strcpy(out, "SENSOR_INSUFFICIENT_BAUD_RATE");
break;
case E_SENSOR_ERROR_BUFFER_OVERFLOW:
strcpy(out, "SENSOR_ERROR_BUFFER_OVERFLOW");
break;
case E_DATA_NOT_ELLIPTICAL:
strcpy(out, "DATA_NOT_ELLIPTICAL");
break;
case E_BOOTLOADER_NONE:
strcpy(out, "No Error: Success, send next record");
break;
case E_BOOTLOADER_INVALID_COMMAND:
strcpy(out, "Invalid Command: Problem with VNX record, abort");
break;
case E_BOOTLOADER_INVALID_RECORD_TYPE:
strcpy(out, "Invalid Record Type: Problem with VNX record, abort");
break;
case E_BOOTLOADER_INVALID_BYTE_COUNT:
strcpy(out, "Invalide Byte Count: Problem with VNX record, abort");
break;
case E_BOOTLOADER_INVALID_MEMORY_ADDRESS:
strcpy(out, "Invalide Memeory Address: Problem with VNX record, abort");
break;
case E_BOOTLOADER_COMM_ERROR:
strcpy(out, "COMM Error: Checksum error, resend record");
break;
case E_BOOTLOADER_INVALID_HEX_FILE:
strcpy(out, "Invalid Hex File: Problem with VNX record, abort");
break;
case E_BOOTLOADER_DECRYPTION_ERROR:
strcpy(out, "Decryption Error: Invalid VNX file or record sent out of order, abort");
break;
case E_BOOTLOADER_INVALID_BLOCK_CRC:
strcpy(out, "Invalide Block CRC: Data verification failed, abort");
break;
case E_BOOTLOADER_INVALID_PROGRAM_CRC:
strcpy(out, "Invalide Program CRC: Problemw ith firmware on device");
break;
case E_BOOTLOADER_INVALID_PROGRAM_SIZE:
strcpy(out, "Invalide Program Size: Problemw ith firmware on device");
break;
case E_BOOTLOADER_MAX_RETRY_COUNT:
strcpy(out, "Max Retry Count: Too many errors, abort");
break;
case E_BOOTLOADER_TIMEOUT:
strcpy(out, "Timeout: Timeout expired, reset");
break;
case E_BOOTLOADER_RESERVED:
strcpy(out, "Reserved: Contact VectorNav, abort");
break;
default:
strcpy(out, "UNKNOWN_VALUE");
break;
}
#if defined(_MSC_VER)
#pragma warning(pop)
#endif
}
src/drivers/ins/vectornav/libvnc/src/vn/error_detection.c
+32
View File
@@ -0,0 +1,32 @@
#include "vn/error_detection.h"
uint8_t VnChecksum8_compute(char const *data, size_t length)
{
uint8_t xorVal = 0;
size_t i;
for (i = 0; i < length; i++)
{
xorVal ^= data[i];
}
return xorVal;
}
uint16_t VnCrc16_compute(char const *data, size_t length)
{
size_t i;
uint16_t crc = 0;
for (i = 0; i < length; i++)
{
crc = (uint16_t) (crc >> 8) | (crc << 8);
crc ^= (uint8_t) data[i];
crc ^= (uint16_t) (((uint8_t) (crc & 0xFF)) >> 4);
crc ^= (uint16_t) ((crc << 8) << 4);
crc ^= (uint16_t) (((crc & 0xFF) << 4) << 1);
}
return crc;
}
src/drivers/ins/vectornav/libvnc/src/vn/math/matrix.c
+123
View File
@@ -0,0 +1,123 @@
#include "vn/math/matrix.h"
#include "vn/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;
}
/*#include <stdio.h>*/
/*vec3f add_v3f_v3f(vec3f lhs, vec3f rhs)
{
vec3f r;
r.x = lhs.x + rhs.x;
r.y = lhs.y + rhs.y;
r.z = lhs.z + rhs.z;
return r;
}
vec3d add_v3d_v3d(vec3d lhs, vec3d rhs)
{
vec3d r;
r.x = lhs.x + rhs.x;
r.y = lhs.y + rhs.y;
r.z = lhs.z + rhs.z;
return r;
}
vec4f add_v4f_v4f(vec4f lhs, vec4f rhs)
{
vec4f r;
r.x = lhs.x + rhs.x;
r.y = lhs.y + rhs.y;
r.z = lhs.z + rhs.z;
r.w = lhs.w + rhs.w;
return r;
}
vec3f sub_v3f_v3f(vec3f lhs, vec3f rhs)
{
vec3f r;
r.x = lhs.x - rhs.x;
r.y = lhs.y - rhs.y;
r.z = lhs.z - rhs.z;
return r;
}
vec3d sub_v3d_v3d(vec3d lhs, vec3d rhs)
{
vec3d r;
r.x = lhs.x - rhs.x;
r.y = lhs.y - rhs.y;
r.z = lhs.z - rhs.z;
return r;
}
vec4f sub_v4f_v4f(vec4f lhs, vec4f rhs)
{
vec4f r;
r.x = lhs.x - rhs.x;
r.y = lhs.y - rhs.y;
r.z = lhs.z - rhs.z;
r.w = lhs.w - rhs.w;
return r;
}*/
#if 0
#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.x, v.y, v.z);
}
void str_vec3d(char* out, vec3d v)
{
sprintf(out, "(%f; %f; %f)", v.x, v.y, v.z);
}
void str_vec4f(char* out, vec4f v)
{
sprintf(out, "(%f; %f; %f; %f)", v.x, v.y, v.z, v.w);
}
#if defined(_MSC_VER)
#pragma warning(pop)
#endif
#endif
src/drivers/ins/vectornav/libvnc/src/vn/math/vector.c
+118
View File
@@ -0,0 +1,118 @@
#include "vn/math/vector.h"
#include <stdio.h>
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
src/drivers/ins/vectornav/libvnc/src/vn/protocol/spi.c
+2911
View File
File diff suppressed because it is too large Load Diff
src/drivers/ins/vectornav/libvnc/src/vn/protocol/upack.c
+4860
View File
File diff suppressed because it is too large Load Diff
src/drivers/ins/vectornav/libvnc/src/vn/protocol/upackf.c
+438
View File
@@ -0,0 +1,438 @@
#include "vn/protocol/upackf.h"
#include <string.h>
/*#include <stdlib.h>
#include <stdio.h>*/
#include "vn/util.h"
/*#define MAXIMUM_REGISTER_ID 255
#define ASCII_START_CHAR '$'
#define BINARY_START_CHAR 0xFA*/
#define ASCII_END_CHAR1 '\r'
#define ASCII_END_CHAR2 '\n'
#define MAX_BINARY_PACKET_SIZE 600
/* Function declarations */
void VnUartPacketFinder_dispatchPacket(VnUartPacketFinder* finder, VnUartPacket* packet, size_t running);
void VnUartPacketFinder_processPacket(VnUartPacketFinder* finder, uint8_t* start, size_t len, size_t running);
void VnUartPacketFinder_resetAllTrackingStatus(VnUartPacketFinder* finder);
void VnUartPacketFinder_resetAsciiStatus(VnUartPacketFinder* finder);
void VnUartPacketFinder_resetBinaryStatus(VnUartPacketFinder* finder);
/** \brief Dispatch packet.
* \param[in] running Running Index. */
void vn_uart_packet_finder_packet_found(VnUartPacketFinder* finder, uint8_t* start, size_t len, size_t running);
char* vnstrtok(char* str, size_t* startIndex);
void VnUartPacketFinder_initialize(VnUartPacketFinder* toInitialize)
{
toInitialize->packetFoundHandler = NULL;
toInitialize->packetFoundHandlerUserData = NULL;
toInitialize->runningDataIndex = 0;
toInitialize->asciiCurrentlyBuildingPacket = false;
toInitialize->asciiPossibleStartOfPacketIndex = 0;
toInitialize->asciiRunningDataIndexOfStart = 0;
toInitialize->asciiEndChar1Found = false;
toInitialize->binaryCurrentlyBuildingBinaryPacket = false;
toInitialize->binaryRunningDataIndexOfStart = 0;
toInitialize->bufferSize = VNUART_PROTOCOL_BUFFER_SIZE;
toInitialize->binaryGroupsPresentFound = false;
toInitialize->binaryGroupsPresent = 0;
toInitialize->binaryNumOfBytesRemainingToHaveAllGroupFields = 0;
toInitialize->binaryPossibleStartIndex = 0;
toInitialize->binaryNumberOfBytesRemainingForCompletePacket = 0;
toInitialize->bufferAppendLocation = 0;
}
VnError VnUartPacketFinder_processData(VnUartPacketFinder* finder, uint8_t* data, size_t len)
{
return VnUartPacketFinder_processData_ex(finder, data, len, false);
}
VnError VnUartPacketFinder_processData_ex(VnUartPacketFinder* finder, uint8_t* data, size_t len, bool bootloaderFilter)
{
bool asciiStartFoundInProvidedBuffer = false;
bool binaryStartFoundInProvidedDataBuffer = false;
size_t i, dataIndexToStartCopyingFrom, binaryDataMoveOverIndexAdjustment;
bool binaryDataToCopyOver;
/* Assume that since the _runningDataIndex is unsigned, any overflows
* will naturally go to zero, which is the behavior that we want. */
for (i = 0; i < len; i++, finder->runningDataIndex++)
{
bool justFinishedBinaryPacket = false;
if ((data[i] == VN_ASCII_START_CHAR) || (bootloaderFilter && (!finder->asciiCurrentlyBuildingPacket && data[i] == VN_BOOTLOAD_START_CHAR)))
{
VnUartPacketFinder_resetAsciiStatus(finder);
finder->asciiCurrentlyBuildingPacket = true;
finder->asciiPossibleStartOfPacketIndex = i;
finder->asciiRunningDataIndexOfStart = finder->runningDataIndex;
asciiStartFoundInProvidedBuffer = true;
}
else if (finder->asciiCurrentlyBuildingPacket && data[i] == ASCII_END_CHAR1)
{
finder->asciiEndChar1Found = true;
}
else if ((bootloaderFilter || (!bootloaderFilter && finder->asciiEndChar1Found)) && (finder->asciiCurrentlyBuildingPacket && data[i] == ASCII_END_CHAR2))
{
if (asciiStartFoundInProvidedBuffer)
{
uint8_t* startOfAsciiPacket;
size_t packetLength;
/* All the packet was in this data buffer so we don't
* need to do any copying. */
startOfAsciiPacket = data + finder->asciiPossibleStartOfPacketIndex;
packetLength = i - finder->asciiPossibleStartOfPacketIndex + 1;
VnUartPacketFinder_processPacket(finder, startOfAsciiPacket, packetLength, finder->asciiRunningDataIndexOfStart);
asciiStartFoundInProvidedBuffer = false;
VnUartPacketFinder_resetAsciiStatus(finder);
}
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 (finder->bufferAppendLocation + i < finder->bufferSize)
{
uint8_t* startOfAsciiPacket;
size_t packetLength;
memcpy(finder->receiveBuffer + finder->bufferAppendLocation, data, i + 1);
startOfAsciiPacket = finder->receiveBuffer + finder->asciiPossibleStartOfPacketIndex;
packetLength = finder->bufferAppendLocation + i + 1 - finder->asciiPossibleStartOfPacketIndex;
VnUartPacketFinder_processPacket(finder, startOfAsciiPacket, packetLength, finder->asciiRunningDataIndexOfStart);
asciiStartFoundInProvidedBuffer = false;
VnUartPacketFinder_resetAsciiStatus(finder);
}
else
{
/* We are about to overflow our buffer. */
VnUartPacketFinder_resetAllTrackingStatus(finder);
asciiStartFoundInProvidedBuffer = false;
binaryStartFoundInProvidedDataBuffer = false;
}
}
}
else if (finder->asciiEndChar1Found)
{
/* Must not be a valid ASCII packet. */
VnUartPacketFinder_resetAsciiStatus(finder);
asciiStartFoundInProvidedBuffer = false;
if (!finder->binaryCurrentlyBuildingBinaryPacket)
finder->bufferAppendLocation = 0;
}
/* Update our binary packet in processing. */
if (finder->binaryCurrentlyBuildingBinaryPacket)
{
if (!finder->binaryGroupsPresentFound)
{
/* This byte must be the groups present. */
finder->binaryGroupsPresentFound = true;
finder->binaryGroupsPresent = (uint8_t) data[i];
finder->binaryNumOfBytesRemainingToHaveAllGroupFields = (uint8_t) (2 * VnUtil_countSetBitsUint8((uint8_t) data[i]));
}
else if (finder->binaryNumOfBytesRemainingToHaveAllGroupFields != 0)
{
/* We found another byte belonging to this possible binary packet. */
finder->binaryNumOfBytesRemainingToHaveAllGroupFields--;
if (finder->binaryNumOfBytesRemainingToHaveAllGroupFields == 0)
{
/* We have all of the group fields now. */
size_t remainingBytesForCompletePacket = 0;
if (binaryStartFoundInProvidedDataBuffer)
{
size_t headerLength = i - finder->binaryPossibleStartIndex + 1;
remainingBytesForCompletePacket = VnUartPacket_computeBinaryPacketLength(data + finder->binaryPossibleStartIndex) - 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 = finder->bufferAppendLocation - finder->binaryPossibleStartIndex + numOfBytesToCopyIntoReceiveBuffer;
if (finder->bufferAppendLocation + numOfBytesToCopyIntoReceiveBuffer < finder->bufferSize)
{
memcpy(
finder->receiveBuffer + finder->bufferAppendLocation,
data,
numOfBytesToCopyIntoReceiveBuffer);
remainingBytesForCompletePacket = VnUartPacket_computeBinaryPacketLength(finder->receiveBuffer + finder->binaryPossibleStartIndex) - headerLength;
}
else
{
/* About to overrun our receive buffer! */
VnUartPacketFinder_resetAllTrackingStatus(finder);
binaryStartFoundInProvidedDataBuffer = false;
asciiStartFoundInProvidedBuffer = false;
}
}
if (finder->binaryCurrentlyBuildingBinaryPacket)
{
if (remainingBytesForCompletePacket > MAX_BINARY_PACKET_SIZE)
{
/* Must be a bad possible binary packet. */
VnUartPacketFinder_resetBinaryStatus(finder);
binaryStartFoundInProvidedDataBuffer = false;
if (!finder->asciiCurrentlyBuildingPacket)
finder->bufferAppendLocation = 0;
}
else
{
finder->binaryNumberOfBytesRemainingForCompletePacket = remainingBytesForCompletePacket;
}
}
}
}
else
{
/* We are currently collecting data for our packet. */
finder->binaryNumberOfBytesRemainingForCompletePacket--;
if (finder->binaryNumberOfBytesRemainingForCompletePacket == 0)
{
/* We have a possible binary packet! */
if (binaryStartFoundInProvidedDataBuffer)
{
uint8_t* packetStart;
size_t packetLength;
/* The binary packet exists completely in the user's provided buffer. */
packetStart = data + finder->binaryPossibleStartIndex;
packetLength = i - finder->binaryPossibleStartIndex + 1;
VnUartPacketFinder_processPacket(finder, packetStart, packetLength, finder->binaryRunningDataIndexOfStart);
justFinishedBinaryPacket = true;
}
else
{
/* The packet is split between our receive buffer and the user's buffer. */
size_t numOfBytesToCopyIntoReceiveBuffer = i + 1;
if (finder->bufferAppendLocation + numOfBytesToCopyIntoReceiveBuffer < finder->bufferSize)
{
uint8_t* packetStart;
size_t packetLength;
memcpy(
finder->receiveBuffer + finder->bufferAppendLocation,
data,
numOfBytesToCopyIntoReceiveBuffer);
packetStart = finder->receiveBuffer + finder->binaryPossibleStartIndex;
packetLength = finder->bufferAppendLocation - finder->binaryPossibleStartIndex + i + 1;
VnUartPacketFinder_processPacket(finder, packetStart, packetLength, finder->binaryRunningDataIndexOfStart);
justFinishedBinaryPacket = true;
}
else
{
/* About to overrun our receive buffer! */
VnUartPacketFinder_resetAllTrackingStatus(finder);
binaryStartFoundInProvidedDataBuffer = false;
asciiStartFoundInProvidedBuffer = false;
}
}
}
}
}
if (data[i] == VN_BINARY_START_CHAR && !justFinishedBinaryPacket)
{
if (!finder->binaryCurrentlyBuildingBinaryPacket)
{
/* Possible start of a binary packet. */
binaryStartFoundInProvidedDataBuffer = true;
finder->binaryCurrentlyBuildingBinaryPacket = true;
finder->binaryPossibleStartIndex = i;
finder->binaryGroupsPresentFound = false;
finder->binaryNumOfBytesRemainingToHaveAllGroupFields = 0;
finder->binaryNumberOfBytesRemainingForCompletePacket = 0;
finder->binaryRunningDataIndexOfStart = finder->runningDataIndex;
}
}
}
/* Perform any data copying to our receive buffer. */
dataIndexToStartCopyingFrom = 0;
binaryDataToCopyOver = false;
binaryDataMoveOverIndexAdjustment = 0;
if (finder->binaryCurrentlyBuildingBinaryPacket)
{
binaryDataToCopyOver = true;
if (binaryStartFoundInProvidedDataBuffer)
{
dataIndexToStartCopyingFrom = finder->binaryPossibleStartIndex;
binaryDataMoveOverIndexAdjustment = dataIndexToStartCopyingFrom;
}
}
if (finder->asciiCurrentlyBuildingPacket && asciiStartFoundInProvidedBuffer)
{
if (finder->asciiPossibleStartOfPacketIndex < dataIndexToStartCopyingFrom)
{
binaryDataMoveOverIndexAdjustment -= binaryDataMoveOverIndexAdjustment - finder->asciiPossibleStartOfPacketIndex;
dataIndexToStartCopyingFrom = finder->asciiPossibleStartOfPacketIndex;
}
else if (!binaryDataToCopyOver)
{
dataIndexToStartCopyingFrom = finder->asciiPossibleStartOfPacketIndex;
}
/* Adjust our ASCII index to be based on the receive buffer. */
finder->asciiPossibleStartOfPacketIndex = finder->bufferAppendLocation + finder->asciiPossibleStartOfPacketIndex - dataIndexToStartCopyingFrom;
}
/* Adjust any binary packet indexes we are currently building. */
if (finder->binaryCurrentlyBuildingBinaryPacket)
{
if (binaryStartFoundInProvidedDataBuffer)
{
finder->binaryPossibleStartIndex = finder->binaryPossibleStartIndex - binaryDataMoveOverIndexAdjustment + finder->bufferAppendLocation;
}
}
if (finder->bufferAppendLocation + len - dataIndexToStartCopyingFrom < finder->bufferSize)
{
/* Safe to copy over the data. */
size_t numOfBytesToCopyOver = len - dataIndexToStartCopyingFrom;
uint8_t *copyFromStart = data + dataIndexToStartCopyingFrom;
memcpy(
finder->receiveBuffer + finder->bufferAppendLocation,
copyFromStart,
numOfBytesToCopyOver);
finder->bufferAppendLocation += numOfBytesToCopyOver;
}
else
{
/* We are about to overflow our buffer. */
VnUartPacketFinder_resetAsciiStatus(finder);
VnUartPacketFinder_resetBinaryStatus(finder);
finder->bufferAppendLocation = 0;
}
return E_NONE;
}
void VnUartPacketFinder_processPacket(VnUartPacketFinder* finder, uint8_t* start, size_t len, size_t running)
{
VnUartPacket p;
PacketType t;
VnUartPacket_initialize(&p, start, len);
t = VnUartPacket_type(&p);
if (VnUartPacket_isValid(&p))
{
VnUartPacketFinder_dispatchPacket(finder, &p, running);
/* Reset tracking for both packet types since this packet was valid. */
VnUartPacketFinder_resetAsciiStatus(finder);
VnUartPacketFinder_resetBinaryStatus(finder);
finder->bufferAppendLocation = 0;
}
else
{
/* Invalid packet! Reset tracking. */
if (t == PACKETTYPE_ASCII)
{
VnUartPacketFinder_resetAsciiStatus(finder);
if (!finder->binaryCurrentlyBuildingBinaryPacket)
finder->bufferAppendLocation = 0;
}
else
{
VnUartPacketFinder_resetBinaryStatus(finder);
if (!finder->asciiCurrentlyBuildingPacket)
finder->bufferAppendLocation = 0;
}
}
}
void VnUartPacketFinder_resetAsciiStatus(VnUartPacketFinder* finder)
{
finder->asciiCurrentlyBuildingPacket = false;
finder->asciiPossibleStartOfPacketIndex = 0;
finder->asciiEndChar1Found = false;
finder->asciiRunningDataIndexOfStart = 0;
}
void VnUartPacketFinder_resetBinaryStatus(VnUartPacketFinder* finder)
{
finder->binaryCurrentlyBuildingBinaryPacket = false;
finder->binaryGroupsPresentFound = false;
finder->binaryGroupsPresent = 0;
finder->binaryNumOfBytesRemainingToHaveAllGroupFields = 0;
finder->binaryPossibleStartIndex = 0;
finder->binaryNumberOfBytesRemainingForCompletePacket = 0;
finder->binaryRunningDataIndexOfStart = 0;
}
void VnUartPacketFinder_resetAllTrackingStatus(VnUartPacketFinder *finder)
{
if (finder->asciiCurrentlyBuildingPacket)
VnUartPacketFinder_resetAsciiStatus(finder);
if (finder->binaryCurrentlyBuildingBinaryPacket)
VnUartPacketFinder_resetBinaryStatus(finder);
finder->bufferAppendLocation = 0;
}
void VnUartPacketFinder_dispatchPacket(VnUartPacketFinder* finder, VnUartPacket* packet, size_t running)
{
if (finder->packetFoundHandler == NULL)
return;
finder->packetFoundHandler(finder->packetFoundHandlerUserData, packet, running);
}
VnError VnUartPacketFinder_registerPacketFoundHandler(VnUartPacketFinder* finder, VnUartPacketFinder_PacketFoundHandler handler, void *userData)
{
if (finder->packetFoundHandler != NULL)
return E_INVALID_OPERATION;
finder->packetFoundHandler = handler;
finder->packetFoundHandlerUserData = userData;
return E_NONE;
}
src/drivers/ins/vectornav/libvnc/src/vn/sensors.c
+3526
View File
File diff suppressed because it is too large Load Diff

Some files were not shown because too many files have changed in this diff Show More