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

Tools/astyle/files_to_check_code_style.sh

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_INS_VECTORNAV
+	bool "vectornav"
+	default n
+	---help---
+		Enable support for vectornav

src/drivers/ins/vectornav/VectorNav.cpp

@@ -0,0 +1,701 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "VectorNav.hpp"
+
+#include <lib/drivers/device/Device.hpp>
+#include <px4_platform_common/getopt.h>
+
+#include <fcntl.h>
+
+VectorNav::VectorNav(const char *port) :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port))
+{
+	// store port name
+	strncpy(_port, port, sizeof(_port) - 1);
+
+	// enforce null termination
+	_port[sizeof(_port) - 1] = '\0';
+
+	device::Device::DeviceId device_id{};
+	device_id.devid_s.devtype = DRV_INS_DEVTYPE_VN300;
+	device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL;
+
+	_px4_accel.set_device_id(device_id.devid);
+	_px4_gyro.set_device_id(device_id.devid);
+	_px4_mag.set_device_id(device_id.devid);
+
+	// uint8_t bus_num = atoi(&_port[strlen(_port) - 1]); // Assuming '/dev/ttySx'
+
+	// if (bus_num < 10) {
+	// 	device_id.devid_s.bus = bus_num;
+	// }
+
+	_attitude_pub.advertise();
+	_local_position_pub.advertise();
+	_global_position_pub.advertise();
+}
+
+VectorNav::~VectorNav()
+{
+	VnSensor_unregisterAsyncPacketReceivedHandler(&_vs);
+	VnSensor_unregisterErrorPacketReceivedHandler(&_vs);
+	VnSensor_disconnect(&_vs);
+
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+}
+
+void VectorNav::binaryAsyncMessageReceived(void *userData, VnUartPacket *packet, size_t runningIndex)
+{
+	if (VnUartPacket_isError(packet)) {
+		uint8_t error = 0;
+		VnUartPacket_parseError(packet, &error);
+
+		char buffer[128] {};
+		strFromSensorError(buffer, (SensorError)error);
+
+		PX4_ERR("%s", buffer);
+
+	} else if (userData && (VnUartPacket_type(packet) == PACKETTYPE_BINARY)) {
+		static_cast<VectorNav *>(userData)->sensorCallback(packet);
+	}
+}
+
+void VectorNav::sensorCallback(VnUartPacket *packet)
+{
+	const hrt_abstime time_now_us = hrt_absolute_time();
+
+	//BinaryGroupType groups = (BinaryGroupType)VnUartPacket_groups(packet);
+
+	//size_t curGroupFieldIndex = 0;
+
+	// low pass offset between VN and hrt
+	// use timestamp for publish and timestamp_sample
+
+	// binary output 1
+	if (VnUartPacket_isCompatible(packet,
+				      COMMONGROUP_NONE,
+				      TIMEGROUP_TIMESTARTUP,
+				      (ImuGroup)(IMUGROUP_UNCOMPACCEL | IMUGROUP_UNCOMPGYRO),
+				      GPSGROUP_NONE,
+				      ATTITUDEGROUP_NONE,
+				      INSGROUP_NONE,
+				      GPSGROUP_NONE)
+	   ) {
+		// TIMEGROUP_TIMESTARTUP
+		//uint64_t time_startup = VnUartPacket_extractUint64(packet);
+
+		// IMUGROUP_UNCOMPACCEL
+		vec3f accel = VnUartPacket_extractVec3f(packet);
+
+		// IMUGROUP_UNCOMPGYRO
+		vec3f angular_rate = VnUartPacket_extractVec3f(packet);
+
+		// publish sensor_accel
+		_px4_accel.update(time_now_us, accel.c[0], accel.c[1], accel.c[2]);
+
+		// publish sensor_gyro
+		_px4_gyro.update(time_now_us, angular_rate.c[0], angular_rate.c[1], angular_rate.c[2]);
+	}
+
+	// binary output 2
+	if (VnUartPacket_isCompatible(packet,
+				      COMMONGROUP_NONE,
+				      TIMEGROUP_TIMESTARTUP,
+				      (ImuGroup)(IMUGROUP_TEMP | IMUGROUP_PRES | IMUGROUP_MAG),
+				      GPSGROUP_NONE,
+				      (AttitudeGroup)(ATTITUDEGROUP_QUATERNION | ATTITUDEGROUP_LINEARACCELNED),
+				      (InsGroup)(INSGROUP_INSSTATUS | INSGROUP_POSLLA | INSGROUP_VELNED | INSGROUP_POSU | INSGROUP_VELU),
+				      GPSGROUP_NONE)
+	   ) {
+		// TIMEGROUP_TIMESTARTUP
+		//const uint64_t time_startup = VnUartPacket_extractUint64(packet);
+
+		// IMUGROUP_TEMP
+		const float temperature = VnUartPacket_extractFloat(packet);
+
+		// IMUGROUP_PRES
+		const float pressure = VnUartPacket_extractFloat(packet) * 1000.f; // kPa -> Pa
+
+		// IMUGROUP_MAG
+		const vec3f mag = VnUartPacket_extractVec3f(packet);
+
+		// ATTITUDEGROUP_QUATERNION
+		const vec4f quaternion = VnUartPacket_extractVec4f(packet);
+
+		// ATTITUDEGROUP_LINEARACCELNED
+		const vec3f accelerationLinearNed = VnUartPacket_extractVec3f(packet);
+
+		// INSGROUP_INSSTATUS
+		const uint16_t insStatus = VnUartPacket_extractUint16(packet);
+
+		// INSGROUP_POSLLA
+		const vec3d positionEstimatedLla = VnUartPacket_extractVec3d(packet);
+
+		// INSGROUP_VELNED
+		const vec3f velocityEstimatedNed = VnUartPacket_extractVec3f(packet);
+
+		// INSGROUP_POSU
+		const float positionUncertaintyEstimated = VnUartPacket_extractFloat(packet);
+
+		// INSGROUP_VELU
+		const float velocityUncertaintyEstimated = VnUartPacket_extractFloat(packet);
+
+
+		// update all temperatures
+		_px4_accel.set_temperature(temperature);
+		_px4_gyro.set_temperature(temperature);
+		_px4_mag.set_temperature(temperature);
+
+		// publish sensor_baro
+		sensor_baro_s sensor_baro{};
+		sensor_baro.device_id = 0; // TODO: DRV_INS_DEVTYPE_VN300;
+		sensor_baro.pressure = pressure;
+		sensor_baro.temperature = temperature;
+		sensor_baro.timestamp = hrt_absolute_time();
+		_sensor_baro_pub.publish(sensor_baro);
+
+		// publish sensor_mag
+		_px4_mag.update(time_now_us, mag.c[0], mag.c[1], mag.c[2]);
+
+		// publish attitude
+		vehicle_attitude_s attitude{};
+		attitude.timestamp_sample = time_now_us;
+		attitude.q[0] = quaternion.c[0];
+		attitude.q[1] = quaternion.c[1];
+		attitude.q[2] = quaternion.c[2];
+		attitude.q[3] = quaternion.c[3];
+		attitude.timestamp = hrt_absolute_time();
+		_attitude_pub.publish(attitude);
+
+		// mode
+		const uint16_t mode = (insStatus & 0b11);
+		//const bool mode_initializing = (mode == 0b00);
+		const bool mode_aligning     = (mode == 0b01);
+		const bool mode_tracking     = (mode == 0b10);
+		const bool mode_loss_gnss    = (mode == 0b11);
+
+
+		// mode_initializing
+		//  - pitch and roll are good (yaw is mag based)
+
+		// mode_aligning.
+		//  - attitude good
+		//  - position and velocity are good
+		//  - heading determined largely by mag and not by GPS
+		//  - heading has high uncertainly (yaw is good for control)
+
+		// mode_tracking
+		//  - heading is good for control
+		//
+
+		// mode_loss_gnss
+		//  - lost for > 45 seconds
+		//  - attitude is good
+		//  - treat as mode 0
+
+
+		if ((mode_aligning || mode_tracking) && !mode_loss_gnss) {
+			// publish local_position
+
+			// TODO: projection
+			vehicle_local_position_s local_position{};
+			local_position.timestamp_sample = time_now_us;
+			local_position.ax = accelerationLinearNed.c[0];
+			local_position.ay = accelerationLinearNed.c[1];
+			local_position.az = accelerationLinearNed.c[2];
+			local_position.x = positionEstimatedLla.c[0]; // TODO
+			local_position.y = positionEstimatedLla.c[1]; // TODO
+			local_position.z = positionEstimatedLla.c[2]; // TODO
+			local_position.vx = velocityEstimatedNed.c[0];
+			local_position.vy = velocityEstimatedNed.c[1];
+			local_position.vz = velocityEstimatedNed.c[2];
+			local_position.eph = positionUncertaintyEstimated;
+			local_position.epv = positionUncertaintyEstimated;
+			local_position.evh = velocityUncertaintyEstimated;
+			local_position.evv = velocityUncertaintyEstimated;
+			local_position.xy_valid = true;
+			local_position.heading_good_for_control = mode_tracking;
+			local_position.timestamp = hrt_absolute_time();
+			_local_position_pub.publish(local_position);
+
+
+
+			// publish global_position
+			vehicle_global_position_s global_position{};
+			global_position.timestamp_sample = time_now_us;
+			global_position.lat = positionEstimatedLla.c[0];
+			global_position.lon = positionEstimatedLla.c[1];
+			global_position.alt = positionEstimatedLla.c[2];
+			global_position.timestamp = hrt_absolute_time();
+			_global_position_pub.publish(global_position);
+		}
+	}
+
+	// binary output 3
+	if (VnUartPacket_isCompatible(packet,
+				      COMMONGROUP_NONE,
+				      TIMEGROUP_TIMESTARTUP,
+				      IMUGROUP_NONE,
+				      (GpsGroup)(GPSGROUP_UTC | GPSGROUP_NUMSATS | GPSGROUP_FIX | GPSGROUP_POSLLA | GPSGROUP_VELNED
+						      | GPSGROUP_POSU | GPSGROUP_VELU | GPSGROUP_DOP),
+				      ATTITUDEGROUP_NONE,
+				      INSGROUP_NONE,
+				      GPSGROUP_NONE)
+	   ) {
+		// TIMEGROUP_TIMESTARTUP
+		//const uint64_t time_startup = VnUartPacket_extractUint64(packet);
+
+		// GPSGROUP_UTC
+		// TimeUtc timeUtc;
+		// timeUtc.year = VnUartPacket_extractInt8(packet);
+		// timeUtc.month = VnUartPacket_extractUint8(packet);
+		// timeUtc.day = VnUartPacket_extractUint8(packet);
+		// timeUtc.hour = VnUartPacket_extractUint8(packet);
+		// timeUtc.min = VnUartPacket_extractUint8(packet);
+		// timeUtc.sec = VnUartPacket_extractUint8(packet);
+		// timeUtc.ms = VnUartPacket_extractUint16(packet);
+
+		// GPSGROUP_NUMSATS
+		const uint8_t numSats = VnUartPacket_extractUint8(packet);
+
+		// GPSGROUP_FIX
+		const uint8_t gpsFix = VnUartPacket_extractUint8(packet);
+
+		// GPSGROUP_POSLLA
+		const vec3d positionGpsLla = VnUartPacket_extractVec3d(packet);
+
+		// GPSGROUP_VELNED
+		const vec3f velocityGpsNed = VnUartPacket_extractVec3f(packet);
+
+		// GPSGROUP_POSU
+		const vec3f positionUncertaintyGpsNed = VnUartPacket_extractVec3f(packet);
+
+		// GPSGROUP_VELU
+		const float velocityUncertaintyGps = VnUartPacket_extractFloat(packet);
+
+		// GPSGROUP_DOP
+		const GpsDop dop = VnUartPacket_extractGpsDop(packet);
+
+
+		// publish sensor_gnss
+		if (gpsFix > 0) {
+			sensor_gps_s sensor_gps{};
+			sensor_gps.timestamp_sample = time_now_us;
+
+			sensor_gps.device_id = 0; // TODO
+
+			sensor_gps.time_utc_usec = 0;
+
+			sensor_gps.satellites_used = numSats;
+
+			sensor_gps.fix_type = gpsFix;
+
+			sensor_gps.lat = positionGpsLla.c[0] * 1e7;
+			sensor_gps.lon = positionGpsLla.c[1] * 1e7;
+			sensor_gps.alt = positionGpsLla.c[2] * 1e3;
+			sensor_gps.alt_ellipsoid = sensor_gps.alt;
+
+			sensor_gps.vel_ned_valid = true;
+			sensor_gps.vel_n_m_s = velocityGpsNed.c[0];
+			sensor_gps.vel_e_m_s = velocityGpsNed.c[1];
+			sensor_gps.vel_d_m_s = velocityGpsNed.c[2];
+
+			sensor_gps.hdop = dop.hDOP;
+			sensor_gps.vdop = dop.vDOP;
+
+			sensor_gps.eph = positionUncertaintyGpsNed.c[0];
+			sensor_gps.epv = positionUncertaintyGpsNed.c[2];
+
+			sensor_gps.s_variance_m_s = velocityUncertaintyGps;
+
+			sensor_gps.timestamp = hrt_absolute_time();
+			_sensor_gps_pub.publish(sensor_gps);
+		}
+	}
+}
+
+bool VectorNav::init()
+{
+	// first try default baudrate
+	const uint32_t DEFAULT_BAUDRATE = 115200;
+	const uint32_t DESIRED_BAUDRATE = 921600;
+
+	// first try default baudrate, if that fails try all other supported baudrates
+	VnSensor_initialize(&_vs);
+
+	if ((VnSensor_connect(&_vs, _port, DEFAULT_BAUDRATE) != E_NONE) || !VnSensor_verifySensorConnectivity(&_vs)) {
+
+		static constexpr uint32_t BAUDRATES[] {9600, 19200, 38400, 57600, 115200, 128000, 230400, 460800, 921600};
+
+		for (auto &baudrate : BAUDRATES) {
+			VnSensor_initialize(&_vs);
+
+			if (VnSensor_connect(&_vs, _port, baudrate) == E_NONE && VnSensor_verifySensorConnectivity(&_vs)) {
+				PX4_DEBUG("found baudrate %d", baudrate);
+				break;
+			}
+
+			// disconnect before trying again
+			VnSensor_disconnect(&_vs);
+		}
+	}
+
+	VnError error = E_NONE;
+
+	// change baudrate to max
+	if ((error = VnSensor_changeBaudrate(&_vs, DESIRED_BAUDRATE)) != E_NONE) {
+		PX4_WARN("Error changing baud rate failed: %d", error);
+	}
+
+	// query the sensor's model number
+	char model_number[30] {};
+
+	if ((error = VnSensor_readModelNumber(&_vs, model_number, sizeof(model_number))) != E_NONE) {
+		PX4_ERR("Error reading model number %d", error);
+		return false;
+	}
+
+	// query the sensor's hardware revision
+	uint32_t hardware_revision = 0;
+
+	if ((error = VnSensor_readHardwareRevision(&_vs, &hardware_revision)) != E_NONE) {
+		PX4_ERR("Error reading HW revision %d", error);
+		return false;
+	}
+
+	// query the sensor's serial number
+	uint32_t serial_number = 0;
+
+	if ((error = VnSensor_readSerialNumber(&_vs, &serial_number)) != E_NONE) {
+		PX4_ERR("Error reading serial number %d", error);
+		return false;
+	}
+
+	// query the sensor's firmware version
+	char firmware_version[30] {};
+
+	if ((error = VnSensor_readFirmwareVersion(&_vs, firmware_version, sizeof(firmware_version))) != E_NONE) {
+		PX4_ERR("Error reading firmware version %d", error);
+		return false;
+	}
+
+	PX4_INFO("Model: %s, HW REV: %" PRIu32 ", SN: %" PRIu32 ", SW VER: %s", model_number, hardware_revision, serial_number,
+		 firmware_version);
+
+	return true;
+}
+
+bool VectorNav::configure()
+{
+	// disable all ASCII messages
+	VnSensor_writeAsyncDataOutputType(&_vs, VNOFF, true);
+
+	VnError error = E_NONE;
+
+	/* For the registers that have more complex configuration options, it is
+	 * convenient to read the current existing register configuration, change
+	 * only the values of interest, and then write the configuration to the
+	 * register. This allows preserving the current settings for the register's
+	 * other fields. Below, we change the heading mode used by the sensor. */
+	VpeBasicControlRegister vpeReg{};
+
+	if (VnSensor_readVpeBasicControl(&_vs, &vpeReg) == E_NONE) {
+
+		char strConversions[30] {};
+		strFromHeadingMode(strConversions, (VnHeadingMode)vpeReg.headingMode);
+		PX4_DEBUG("Old Heading Mode: %s\n", strConversions);
+
+		vpeReg.enable = VNVPEENABLE_ENABLE;
+		vpeReg.headingMode = VNHEADINGMODE_ABSOLUTE;
+
+		if (VnSensor_writeVpeBasicControl(&_vs, vpeReg, true) == E_NONE) {
+
+			if (VnSensor_readVpeBasicControl(&_vs, &vpeReg) == E_NONE) {
+				strFromHeadingMode(strConversions, (VnHeadingMode)vpeReg.headingMode);
+				PX4_DEBUG("New Heading Mode: %s\n", strConversions);
+			}
+
+		} else {
+			PX4_ERR("Error writing VPE basic control");
+		}
+
+	} else {
+		PX4_ERR("Error reading VPE basic control %d", error);
+	}
+
+
+	VnError VnSensor_readImuFilteringConfiguration(VnSensor * sensor, ImuFilteringConfigurationRegister * fields);
+	// VnError VnSensor_writeImuFilteringConfiguration(VnSensor *sensor, ImuFilteringConfigurationRegister fields, bool waitForReply);
+
+	// grab ImuFilteringConfigurationRegister, zero all modes and write
+
+
+
+	// TODO:
+	// sensor to antenna
+
+	// VnError VnSensor_readGpsAntennaOffset(VnSensor *s, vec3f *position)
+	// VnSensor_writeGpsAntennaOffset
+
+	// A -> B
+	//   GpsCompassBaselineRegister
+	//    position and uncertainty
+	// VnSensor_readGpsCompassBaseline
+
+
+	// binary output 1: max rate IMU
+	BinaryOutputRegister_initialize(
+		&_binary_output_group_1,
+		ASYNCMODE_PORT1,
+		1, // divider
+		COMMONGROUP_NONE,
+		TIMEGROUP_TIMESTARTUP,
+		(ImuGroup)(IMUGROUP_UNCOMPACCEL | IMUGROUP_UNCOMPGYRO),
+		GPSGROUP_NONE,
+		ATTITUDEGROUP_NONE,
+		INSGROUP_NONE,
+		GPSGROUP_NONE
+	);
+
+	if ((error = VnSensor_writeBinaryOutput1(&_vs, &_binary_output_group_1, true)) != E_NONE) {
+
+		// char buffer[128]{};
+		// strFromVnError((char*)buffer, error);
+		// PX4_ERR("Error writing binary output 1: %s", buffer);
+		PX4_ERR("Error writing binary output 1 %d", error);
+		return false;
+	}
+
+	// binary output 2: medium rate AHRS, INS, baro, mag
+	BinaryOutputRegister_initialize(
+		&_binary_output_group_2,
+		ASYNCMODE_PORT1,
+		8, // divider
+		COMMONGROUP_NONE,
+		TIMEGROUP_TIMESTARTUP,
+		(ImuGroup)(IMUGROUP_TEMP | IMUGROUP_PRES | IMUGROUP_MAG),
+		GPSGROUP_NONE,
+		(AttitudeGroup)(ATTITUDEGROUP_QUATERNION | ATTITUDEGROUP_LINEARACCELNED),
+		(InsGroup)(INSGROUP_INSSTATUS | INSGROUP_POSLLA | INSGROUP_VELNED | INSGROUP_POSU | INSGROUP_VELU),
+		GPSGROUP_NONE
+	);
+
+	if ((error = VnSensor_writeBinaryOutput2(&_vs, &_binary_output_group_2, true)) != E_NONE) {
+		PX4_ERR("Error writing binary output 2 %d", error);
+		return false;
+	}
+
+	// binary output 3: low rate GNSS
+	BinaryOutputRegister_initialize(
+		&_binary_output_group_3,
+		ASYNCMODE_PORT1,
+		80, // divider
+		COMMONGROUP_NONE,
+		TIMEGROUP_TIMESTARTUP,
+		IMUGROUP_NONE,
+		(GpsGroup)(GPSGROUP_UTC | GPSGROUP_NUMSATS | GPSGROUP_FIX | GPSGROUP_POSLLA | GPSGROUP_VELNED
+			   | GPSGROUP_POSU | GPSGROUP_VELU | GPSGROUP_DOP),
+		ATTITUDEGROUP_NONE,
+		INSGROUP_NONE,
+		GPSGROUP_NONE
+	);
+
+	if ((error = VnSensor_writeBinaryOutput3(&_vs, &_binary_output_group_3, true)) != E_NONE) {
+		PX4_ERR("Error writing binary output 3 %d", error);
+		//return false;
+	}
+
+	VnSensor_registerAsyncPacketReceivedHandler(&_vs, VectorNav::binaryAsyncMessageReceived, this);
+	VnSensor_registerErrorPacketReceivedHandler(&_vs, VectorNav::binaryAsyncMessageReceived, this);
+
+	return true;
+}
+
+void VectorNav::Run()
+{
+	if (should_exit()) {
+		VnSensor_unregisterAsyncPacketReceivedHandler(&_vs);
+		VnSensor_disconnect(&_vs);
+		exit_and_cleanup();
+		return;
+
+	} else if (!_initialized) {
+
+		if (!_connected) {
+			if (init()) {
+				_connected = true;
+			}
+		}
+
+		if (_connected) {
+			if (!_configured) {
+				if (configure()) {
+					_configured = true;
+				}
+			}
+		}
+
+		if (_connected && _configured) {
+			_initialized = true;
+
+		} else {
+			ScheduleDelayed(3_s);
+			return;
+		}
+	}
+
+
+
+
+	ScheduleDelayed(100_ms);
+}
+
+int VectorNav::print_status()
+{
+	printf("Using port '%s'\n", _port);
+
+	// if (_device[0] != '\0') {
+	// 	PX4_INFO("UART device: %s", _device);
+	// 	PX4_INFO("UART RX bytes: %"  PRIu32, _bytes_rx);
+	// }
+
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+
+	return 0;
+}
+
+int VectorNav::task_spawn(int argc, char *argv[])
+{
+	bool error_flag = false;
+
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+	const char *device_name = nullptr;
+
+	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'd':
+			device_name = myoptarg;
+			break;
+
+		case '?':
+			error_flag = true;
+			break;
+
+		default:
+			PX4_WARN("unrecognized flag");
+			error_flag = true;
+			break;
+		}
+	}
+
+	if (error_flag) {
+		return -1;
+	}
+
+	if (device_name && (access(device_name, R_OK | W_OK) == 0)) {
+		VectorNav *instance = new VectorNav(device_name);
+
+		if (instance == nullptr) {
+			PX4_ERR("alloc failed");
+			return PX4_ERROR;
+		}
+
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		instance->ScheduleNow();
+
+		return PX4_OK;
+
+	} else {
+		if (device_name) {
+			PX4_ERR("invalid device (-d) %s", device_name);
+
+		} else {
+			PX4_INFO("valid device required");
+		}
+	}
+
+	return PX4_ERROR;
+}
+
+int VectorNav::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int VectorNav::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+
+Serial bus driver for the VectorNav VN-100, VN-200, VN-300.
+
+Most boards are configured to enable/start the driver on a specified UART using the SENS_VN_CFG parameter.
+
+Setup/usage information: https://docs.px4.io/master/en/sensor/vectornav.html
+
+### Examples
+
+Attempt to start driver on a specified serial device.
+$ vectornav start -d /dev/ttyS1
+Stop driver
+$ vectornav stop
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("vectornav", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("ins");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
+	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
+	PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Driver status");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Print driver status");
+
+	return PX4_OK;
+}
+
+extern "C" __EXPORT int vectornav_main(int argc, char *argv[])
+{
+	return VectorNav::main(argc, argv);
+}

src/drivers/ins/vectornav/VectorNav.hpp

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -0,0 +1,670 @@
+#ifndef VNENUM_H_INCLUDED
+#define VNENUM_H_INCLUDED
+
+/** \brief The different types of UART packets. */
+typedef enum
+{
+	PACKETTYPE_BINARY,		/**< Binary packet. */
+	PACKETTYPE_ASCII,		/**< ASCII packet. */
+	PACKETTYPE_UNKNOWN		/**< Unknown packet type. */
+} PacketType;
+
+/** \brief The available binary output groups. */
+typedef enum
+{
+	BINARYGROUPTYPE_COMMON = 0x01,		/**< Common group. */
+	BINARYGROUPTYPE_TIME = 0x02,		/**< Time group. */
+	BINARYGROUPTYPE_IMU = 0x04,			/**< IMU group. */
+	BINARYGROUPTYPE_GPS = 0x08,			/**< GPS group. */
+	BINARYGROUPTYPE_ATTITUDE = 0x10,	/**< Attitude group. */
+	BINARYGROUPTYPE_INS = 0x20,			/**< INS group. */
+	BINARYGROUPTYPE_GPS2 = 0x40			/**< GPS2 group. */
+} BinaryGroupType;
+
+/** \brief Async modes for the Binary Output registers. */
+typedef enum
+{
+	ASYNCMODE_NONE	= 0,	/**< None. */
+	ASYNCMODE_PORT1	= 1,	/**< Serial port 1. */
+	ASYNCMODE_PORT2	= 2,	/**< Serial port 2. */
+	ASYNCMODE_BOTH	= 3		/**< Both serial ports. */
+} AsyncMode;
+
+/** \brief Flags for the binary group 1 'Common' in the binary output registers. */
+typedef enum
+{
+	COMMONGROUP_NONE				= 0x0000,	/**< None. */
+	COMMONGROUP_TIMESTARTUP = 0x0001,	/**< TimeStartup. */
+	COMMONGROUP_TIMEGPS				= 0x0002,	/**< TimeGps. */
+	COMMONGROUP_TIMESYNCIN			= 0x0004,	/**< TimeSyncIn. */
+	COMMONGROUP_YAWPITCHROLL		= 0x0008,	/**< YawPitchRoll. */
+	COMMONGROUP_QUATERNION			= 0x0010,	/**< Quaternion. */
+	COMMONGROUP_ANGULARRATE			= 0x0020,	/**< AngularRate. */
+	COMMONGROUP_POSITION			= 0x0040,	/**< Position. */
+	COMMONGROUP_VELOCITY			= 0x0080,	/**< Velocity. */
+	COMMONGROUP_ACCEL				= 0x0100,	/**< Accel. */
+	COMMONGROUP_IMU					= 0x0200,	/**< Imu. */
+	COMMONGROUP_MAGPRES				= 0x0400,	/**< MagPres. */
+	COMMONGROUP_DELTATHETA			= 0x0800,	/**< DeltaTheta. */
+	COMMONGROUP_INSSTATUS			= 0x1000,	/**< InsStatus. */
+	COMMONGROUP_SYNCINCNT			= 0x2000,	/**< SyncInCnt. */
+	COMMONGROUP_TIMEGPSPPS			= 0x4000	/**< TimeGpsPps. */
+} CommonGroup;
+
+/** \brief Flags for the binary group 2 'Time' in the binary output registers. */
+typedef enum
+{
+	TIMEGROUP_NONE					= 0x0000,	/**< None. */
+	TIMEGROUP_TIMESTARTUP			= 0x0001,	/**< TimeStartup. */
+	TIMEGROUP_TIMEGPS				= 0x0002,	/**< TimeGps. */
+	TIMEGROUP_GPSTOW				= 0x0004,	/**< GpsTow. */
+	TIMEGROUP_GPSWEEK				= 0x0008,	/**< GpsWeek. */
+	TIMEGROUP_TIMESYNCIN			= 0x0010,	/**< TimeSyncIn. */
+	TIMEGROUP_TIMEGPSPPS			= 0x0020,	/**< TimeGpsPps. */
+	TIMEGROUP_TIMEUTC				= 0x0040,	/**< TimeUTC. */
+	TIMEGROUP_SYNCINCNT				= 0x0080,	/**< SyncInCnt. */
+	TIMEGROUP_SYNCOUTCNT				= 0x0100,	/**< SyncOutCnt. */
+	TIMEGROUP_TIMESTATUS				= 0x0200	/**< TimeStatus. */
+} TimeGroup;
+
+/** \brief Flags for the binary group 3 'IMU' in the binary output registers. */
+typedef enum
+{
+	IMUGROUP_NONE					= 0x0000,	/**< None. */
+	IMUGROUP_IMUSTATUS				= 0x0001,	/**< ImuStatus. */
+	IMUGROUP_UNCOMPMAG				= 0x0002,	/**< UncompMag. */
+	IMUGROUP_UNCOMPACCEL			= 0x0004,	/**< UncompAccel. */
+	IMUGROUP_UNCOMPGYRO				= 0x0008,	/**< UncompGyro. */
+	IMUGROUP_TEMP					= 0x0010,	/**< Temp. */
+	IMUGROUP_PRES					= 0x0020,	/**< Pres. */
+	IMUGROUP_DELTATHETA				= 0x0040,	/**< DeltaTheta. */
+	IMUGROUP_DELTAVEL				= 0x0080,	/**< DeltaVel. */
+	IMUGROUP_MAG					= 0x0100,	/**< Mag. */
+	IMUGROUP_ACCEL					= 0x0200,	/**< Accel. */
+	IMUGROUP_ANGULARRATE			= 0x0400,	/**< AngularRate. */
+	IMUGROUP_SENSSAT				= 0x0800	/**< SensSat. */
+	#ifdef EXTRA
+	,
+	IMUGROUP_RAW					= 0x1000	/**< Raw. */
+	#endif
+} ImuGroup;
+
+/** \brief Flags for the binary group 4 'GPS' in the binary output registers. */
+typedef enum
+{
+	GPSGROUP_NONE					= 0x0000,	/**< None. */
+	GPSGROUP_UTC					= 0x0001,	/**< UTC. */
+	GPSGROUP_TOW					= 0x0002,	/**< Tow. */
+	GPSGROUP_WEEK					= 0x0004,	/**< Week. */
+	GPSGROUP_NUMSATS				= 0x0008,	/**< NumSats. */
+	GPSGROUP_FIX					= 0x0010,	/**< Fix. */
+	GPSGROUP_POSLLA					= 0x0020,	/**< PosLla. */
+	GPSGROUP_POSECEF				= 0x0040,	/**< PosEcef. */
+	GPSGROUP_VELNED					= 0x0080,	/**< VelNed. */
+	GPSGROUP_VELECEF				= 0x0100,	/**< VelEcef. */
+	GPSGROUP_POSU					= 0x0200,	/**< PosU. */
+	GPSGROUP_VELU					= 0x0400,	/**< VelU. */
+	GPSGROUP_TIMEU					= 0x0800,	/**< TimeU. */
+  GPSGROUP_TIMEINFO       = 0x1000,	/**< TimeInfo. */
+  GPSGROUP_DOP            = 0x2000	/**< DOP. */
+
+	#ifdef EXTRA
+	,
+	GPSGROUP_SVSTAT					= 0x1000	/**< SvStat. */
+	#endif
+} GpsGroup;
+
+/** \brief Flags for the binary group 5 'Attitude' in the binary output registers. */
+typedef enum
+{
+	ATTITUDEGROUP_NONE				= 0x0000,	/**< None. */
+	ATTITUDEGROUP_VPESTATUS			= 0x0001,	/**< VpeStatus. */
+	ATTITUDEGROUP_YAWPITCHROLL		= 0x0002,	/**< YawPitchRoll. */
+	ATTITUDEGROUP_QUATERNION		= 0x0004,	/**< Quaternion. */
+	ATTITUDEGROUP_DCM				= 0x0008,	/**< DCM. */
+	ATTITUDEGROUP_MAGNED			= 0x0010,	/**< MagNed. */
+	ATTITUDEGROUP_ACCELNED			= 0x0020,	/**< AccelNed. */
+	ATTITUDEGROUP_LINEARACCELBODY	= 0x0040,	/**< LinearAccelBody. */
+	ATTITUDEGROUP_LINEARACCELNED	= 0x0080,	/**< LinearAccelNed. */
+	ATTITUDEGROUP_YPRU				= 0x0100	/**< YprU. */
+	#ifdef EXTRA
+	,
+	ATTITUDEGROUP_YPRRATE			= 0x0200,	/**< YprRate. */
+	ATTITUDEGROUP_STATEAHRS			= 0x0400,	/**< StateAhrs. */
+	ATTITUDEGROUP_COVAHRS			= 0x0800	/**< CovAhrs. */
+	#endif
+} AttitudeGroup;
+
+/** \brief Flags for the binary group 6 'INS' in the binary output registers. */
+typedef enum
+{
+	INSGROUP_NONE					= 0x0000,	/**< None. */
+	INSGROUP_INSSTATUS				= 0x0001,	/**< InsStatus. */
+	INSGROUP_POSLLA					= 0x0002,	/**< PosLla. */
+	INSGROUP_POSECEF				= 0x0004,	/**< PosEcef. */
+	INSGROUP_VELBODY				= 0x0008,	/**< VelBody. */
+	INSGROUP_VELNED					= 0x0010,	/**< VelNed. */
+	INSGROUP_VELECEF				= 0x0020,	/**< VelEcef. */
+	INSGROUP_MAGECEF				= 0x0040,	/**< MagEcef. */
+	INSGROUP_ACCELECEF				= 0x0080,	/**< AccelEcef. */
+	INSGROUP_LINEARACCELECEF		= 0x0100,	/**< LinearAccelEcef. */
+	INSGROUP_POSU					= 0x0200,	/**< PosU. */
+	INSGROUP_VELU					= 0x0400	/**< VelU. */
+	#ifdef EXTRA
+	,
+	INSGROUP_STATEINS				= 0x0800,	/**< StateIns. */
+	INSGROUP_COVINS					= 0x1000	/**< CovIns. */
+	#endif
+} InsGroup;
+
+/** \brief Enumeration of the velocity types */
+typedef enum
+{
+	CDVEL_None,
+	CDVEL_GpsNed,
+	CDVEL_GpsEcef,
+	CDVEL_EstimatedNed,
+	CDVEL_EstimatedEcef,
+	CDVEL_EstimatedBody
+} VelocityType;
+
+/** \brief Errors that the VectorNav sensor can report. */
+typedef enum
+{
+	ERR_HARD_FAULT				= 1,	/**< Hard fault. */
+	ERR_SERIAL_BUFFER_OVERFLOW	= 2,	/**< Serial buffer overflow. */
+	ERR_INVALID_CHECKSUM		= 3,	/**< Invalid checksum. */
+	ERR_INVALID_COMMAND			= 4,	/**< Invalid command. */
+	ERR_NOT_ENOUGH_PARAMETERS	= 5,	/**< Not enough parameters. */
+	ERR_TOO_MANY_PARAMETERS		= 6,	/**< Too many parameters. */
+	ERR_INVALID_PARAMETER		= 7,	/**< Invalid parameter. */
+	ERR_INVALID_REGISTER		= 8,	/**< Invalid register. */
+	ERR_UNAUTHORIZED_ACCESS		= 9,	/**< Unauthorized access. */
+	ERR_WATCHDOG_RESET			= 10,	/**< Watchdog reset. */
+	ERR_OUTPUT_BUFFER_OVERFLOW	= 11,	/**< Output buffer overflow. */
+	ERR_INSUFFICIENT_BAUD_RATE	= 12,	/**< Insufficient baud rate. */
+	ERR_ERROR_BUFFER_OVERFLOW	= 255	/**< Error buffer overflow. */
+} SensorError;
+
+/** \brief Enumeration of the various error messages used by the library. */
+typedef enum
+{
+	/** Indicates there were no errors encountered. */
+	E_NONE,
+
+	/** Indicates an unknown error occurred. */
+	E_UNKNOWN,
+
+	/** Indicates a provided buffer was too small to complete the action. */
+	E_BUFFER_TOO_SMALL,
+
+	/** Indicates a provided value is not valid. */
+	E_INVALID_VALUE,
+
+	/** Indicates the requested functionality is currently not implemented. */
+	E_NOT_IMPLEMENTED,
+
+	/** Indicates the requested functionality is not supported. */
+	E_NOT_SUPPORTED,
+
+	/** Indicates the requested item was not found. */
+	E_NOT_FOUND,
+
+	/** Indicates the operation timed out. */
+	E_TIMEOUT,
+
+	/** Indicates insufficient permission to perform the operation. */
+	E_PERMISSION_DENIED,
+
+	/** Indicates an invalid operation was attempted. */
+	E_INVALID_OPERATION,
+
+	/** Indicates an event was signaled. */
+	E_SIGNALED,
+
+    /** Indicates either not enough memory is available or no memory was allocated */
+    E_MEMORY_NOT_ALLOCATED,
+
+	/** VectorNav sensor hard fault (Code 1). */
+	E_SENSOR_HARD_FAULT = 1001,
+
+	/** VectorNav sensor serial buffer overflow (Code 2). */
+	E_SENSOR_SERIAL_BUFFER_OVERFLOW = 1002,
+
+	/** VectorNav sensor invalid checksum (Code 3). */
+	E_SENSOR_INVALID_CHECKSUM = 1003,
+
+	/** VectorNav sensor invalid command (Code 4). */
+	E_SENSOR_INVALID_COMMAND = 1004,
+
+	/** VectorNav sensor not enough parameters (Code 5). */
+	E_SENSOR_NOT_ENOUGH_PARAMETERS = 1005,
+
+	/** VectorNav sensor too many parameters (Code 6). */
+	E_SENSOR_TOO_MANY_PARAMETERS = 1006,
+
+	/** VectorNav sensor invalid parameter (Code 7). */
+	E_SENSOR_INVALID_PARAMETER = 1007,
+
+	/** VectorNav sensor invalid register (Code 8). */
+	E_SENSOR_INVALID_REGISTER = 1008,
+
+	/** VectorNav sensor unauthorized access (Code 9). */
+	E_SENSOR_UNAUTHORIZED_ACCESS = 1009,
+
+	/** VectorNav sensor watchdog reset (Code 10). */
+	E_SENSOR_WATCHDOG_RESET = 1010,
+
+	/** VectorNav sensor output buffer overflow (Code 11). */
+	E_SENSOR_OUTPUT_BUFFER_OVERFLOW = 1011,
+
+	/** VectorNav sensor insufficient baud rate (Code 12). */
+	E_SENSOR_INSUFFICIENT_BAUD_RATE = 1012,
+
+	/** VectorNav sensor error buffer overflow (Code 13). */
+	E_SENSOR_ERROR_BUFFER_OVERFLOW = 1013,
+
+	E_DATA_NOT_ELLIPTICAL		= 2001,	/**< \brief Data set not elliptical. */
+	E_ILL_CONDITIONED			= 2002,	/**< \brief Algorithm had a bad condition. */
+	E_EXCEEDED_MAX_ITERATIONS	= 2003,	/**< \brief Algorithm exceeded the number of interations it is allowed. */
+	E_BAD_FINAL_INTERATION		= 2004,	/**< \brief Algorithm's last interation changed exceeded threshold. */
+	E_INSUFFICIENT_DATA			= 2005,	/**< \brief Not enough data points were provided. */
+
+	/** \brief Errors that the VectorNav bootload can report. */
+	E_BOOTLOADER_NONE					= 3000,	/**< No Error, send next record*/
+	E_BOOTLOADER_INVALID_COMMAND		= 3001,	/**< Problem with VNX record, abort */
+	E_BOOTLOADER_INVALID_RECORD_TYPE	= 3002,	/**< Problem with VNX record, abort */
+	E_BOOTLOADER_INVALID_BYTE_COUNT		= 3003,	/**< Problem with VNX record, abort */
+	E_BOOTLOADER_INVALID_MEMORY_ADDRESS	= 3004,	/**< Problem with VNX record, abort */
+	E_BOOTLOADER_COMM_ERROR				= 3005,	/**< Checksum error, resend record */
+	E_BOOTLOADER_INVALID_HEX_FILE		= 3006,	/**< Problem with VNX record, abort */
+	E_BOOTLOADER_DECRYPTION_ERROR		= 3007,	/**< Invalid VNX file or record sent out of order, abort */
+	E_BOOTLOADER_INVALID_BLOCK_CRC		= 3008,	/**< Data verification failed, abort */
+	E_BOOTLOADER_INVALID_PROGRAM_CRC	= 3009,	/**< Problemw ith firmware on device */
+	E_BOOTLOADER_INVALID_PROGRAM_SIZE	= 3010,	/**< Problemw ith firmware on device */
+	E_BOOTLOADER_MAX_RETRY_COUNT		= 3011,	/**< Too many errors, abort */
+	E_BOOTLOADER_TIMEOUT				= 3012,	/**< Timeout expired, reset */
+	E_BOOTLOADER_RESERVED				= 3013	/**< Contact VectorNav, abort */
+
+} VnError;
+
+/** \brief Enumeration of the various error-detection algorithms used by the
+ * library. */
+typedef enum
+{
+	/** Signifies no error-detection should be performed. */
+	VNERRORDETECTIONMODE_NONE,
+
+	/** Signifies to use 8-bit XOR checksum. */
+	VNERRORDETECTIONMODE_CHECKSUM,
+
+	/** Signifies to use CRC16-CCITT algorithm. */
+	VNERRORDETECTIONMODE_CRC
+
+} VnErrorDetectionMode;
+
+/** \brief Different modes for the SyncInMode field of the Synchronization Control register. */
+typedef enum
+{
+	#ifdef EXTRA
+	/** \brief Count the number of trigger events on SYNC_IN_2 pin.
+	/ * \deprecated This option is obsolete for VN-100 firmware version 2.0 and greater and VN-200 firmware version 1.0 and greater.
+	 */
+	VNSYNCINMODE_COUNT2 = 0,
+	/** \brief Start ADC sampling on trigger of SYNC_IN_2 pin.
+	/ * \deprecated This option is obsolete for VN-100 firmware version 2.0 and greater and VN-200 firmware version 1.0 and greater.
+	 */
+	VNSYNCINMODE_ADC2 = 1,
+	/** \brief Output asynchronous message on trigger of SYNC_IN_2 pin.
+	/ * \deprecated This option is obsolete for VN-100 firmware version 2.0 and greater and VN-200 firmware version 1.0 and greater.
+	 */
+	VNSYNCINMODE_ASYNC2 = 2,
+	#endif
+	/** \brief Count number of trigger events on SYNC_IN pin.
+	 */
+	VNSYNCINMODE_COUNT = 3,
+	/** \brief Start IMU sampling on trigger of SYNC_IN pin.
+	 */
+	VNSYNCINMODE_IMU = 4,
+	/** \brief Output asynchronous message on trigger of SYNC_IN pin.
+	 */
+	VNSYNCINMODE_ASYNC = 5
+} VnSyncInMode;
+
+/** \brief Different modes for the SyncInEdge field of the Synchronization Control register. */
+typedef enum
+{
+	/** \brief Trigger on the rising edge on the SYNC_IN pin.
+	 */
+	VNSYNCINEDGE_RISING = 0,
+	/** \brief Trigger on the falling edge on the SYNC_IN pin.
+	 */
+	VNSYNCINEDGE_FALLING = 1
+} VnSyncInEdge;
+
+/** \brief Different modes for the SyncOutMode field of the Synchronization Control register. */
+typedef enum
+{
+	/** \brief None.
+	 */
+	VNSYNCOUTMODE_NONE = 0,
+	/** \brief Trigger at start of IMU sampling.
+	 */
+	VNSYNCOUTMODE_ITEMSTART = 1,
+	/** \brief Trigger when IMU measurements are available.
+	 */
+	VNSYNCOUTMODE_IMUREADY = 2,
+	/** \brief Trigger when attitude measurements are available.
+	 */
+	VNSYNCOUTMODE_INS = 3,
+	/** \brief Trigger on GPS PPS event when a 3D fix is valid.
+	 */
+	VNSYNCOUTMODE_GPSPPS = 6
+} VnSyncOutMode;
+
+/** \brief Different modes for the SyncOutPolarity field of the Synchronization Control register. */
+typedef enum
+{
+	/** \brief Negative pulse.
+	 */
+	VNSYNCOUTPOLARITY_NEGATIVE = 0,
+	/** \brief Positive pulse.
+	 */
+	VNSYNCOUTPOLARITY_POSITIVE = 1
+} VnSyncOutPolarity;
+
+/** \brief Counting modes for the Communication Protocol Control register. */
+typedef enum
+{
+	/** \brief Off.
+	 */
+	VNCOUNTMODE_NONE = 0,
+	/** \brief SyncIn counter.
+	 */
+	VNCOUNTMODE_SYNCINCOUNT = 1,
+	/** \brief SyncIn time.
+	 */
+	VNCOUNTMODE_SYNCINTIME = 2,
+	/** \brief SyncOut counter.
+	 */
+	VNCOUNTMODE_SYNCOUTCOUNTER = 3,
+	/** \brief GPS PPS time.
+	 */
+	VNCOUNTMODE_GPSPPS = 4
+} VnCountMode;
+
+/** \brief Status modes for the Communication Protocol Control register. */
+typedef enum
+{
+	/** \brief Off.
+	 */
+	VNSTATUSMODE_OFF = 0,
+	/** \brief VPE status.
+	 */
+	VNSTATUSMODE_VPESTATUS = 1,
+	/** \brief INS status.
+	 */
+	VNSTATUSMODE_INSSTATUS = 2
+} VnStatusMode;
+
+/** \brief Checksum modes for the Communication Protocol Control register. */
+typedef enum
+{
+	/** \brief Off.
+	 */
+	VNCHECKSUMMODE_OFF = 0,
+	/** \brief 8-bit checksum.
+	 */
+	VNCHECKSUMMODE_CHECKSUM = 1,
+	/** \brief 16-bit CRC.
+	 */
+	VNCHECKSUMMODE_CRC = 2
+} VnChecksumMode;
+
+/** \brief Error modes for the Communication Protocol Control register. */
+typedef enum
+{
+	/** \brief Ignore error.
+	 */
+	VNERRORMODE_IGNORE = 0,
+	/** \brief Send error.
+	 */
+	VNERRORMODE_SEND = 1,
+	/** \brief Send error and set ADOR register to off.
+	 */
+	VNERRORMODE_SENDANDOFF = 2
+} VnErrorMode;
+
+/** \brief Filter modes for the IMU Filtering Configuration register. */
+typedef enum
+{
+	/** \brief No filtering.
+	 */
+	VNFILTERMODE_NOFILTERING = 0,
+	/** \brief Filtering performed only on raw uncompensated IMU measurements.
+	 */
+	VNFILTERMODE_ONLYRAW = 1,
+	/** \brief Filtering performed only on compensated IMU measurements.
+	 */
+	VNFILTERMODE_ONLYCOMPENSATED = 2,
+	/** \brief Filtering performed on both uncompensated and compensated IMU measurements.
+	 */
+	VNFILTERMODE_BOTH = 3
+} VnFilterMode;
+
+/** \brief Integration frames for the Delta Theta and Delta Velocity Configuration register. */
+typedef enum
+{
+	/** \brief Body frame.
+	 */
+	VNINTEGRATIONFRAME_BODY = 0,
+	/** \brief NED frame.
+	 */
+	VNINTEGRATIONFRAME_NED = 1
+} VnIntegrationFrame;
+
+/** \brief Compensation modes for the Delta Theta and Delta Velocity configuration register. */
+typedef enum
+{
+	/** \brief None.
+	 */
+	VNCOMPENSATIONMODE_NONE = 0,
+	/** \brief Bias.
+	 */
+	VNCOMPENSATIONMODE_BIAS = 1
+} VnCompensationMode;
+
+/** \brief GPS fix modes for the GPS Solution - LLA register. */
+typedef enum
+{
+	/** \brief No fix.
+	 */
+	VNGPSFIX_NOFIX = 0,
+	/** \brief Time only.
+	 */
+	VNGPSFIX_TIMEONLY = 1,
+	/** \brief 2D.
+	 */
+	VNGPSFIX_2D = 2,
+	/** \brief 3D.
+	 */
+	VNGPSFIX_3D = 3
+} VnGpsFix;
+
+/** \brief GPS modes for the GPS Configuration register. */
+typedef enum
+{
+	/** \brief Use onboard GPS.
+	 */
+	VNGPSMODE_ONBOARDGPS = 0,
+	/** \brief Use external GPS.
+	 */
+	VNGPSMODE_EXTERNALGPS = 1,
+	/** \brief Use external VN-200 as GPS.
+	 */
+	VNGPSMODE_EXTERNALVN200GPS = 2
+} VnGpsMode;
+
+/** \brief GPS PPS mode for the GPS Configuration register. */
+typedef enum
+{
+	/** \brief GPS PPS signal on GPS_PPS pin and triggered on rising edge.
+	 */
+	VNPPSSOURCE_GPSPPSRISING = 0,
+	/** \brief GPS PPS signal on GPS_PPS pin and triggered on falling edge.
+	 */
+	VNPPSSOURCE_GPSPPSFALLING = 1,
+	/** \brief GPS PPS signal on SyncIn pin and triggered on rising edge.
+	 */
+	VNPPSSOURCE_SYNCINRISING = 2,
+	/** \brief GPS PPS signal on SyncIn pin and triggered on falling edge.
+	 */
+	VNPPSSOURCE_SYNCINFALLING = 3
+} VnPpsSource;
+
+/** \brief VPE Enable mode for the VPE Basic Control register. */
+typedef enum
+{
+	/** \brief Disable
+	 */
+	VNVPEENABLE_DISABLE = 0,
+	/** \brief Enable
+	 */
+	VNVPEENABLE_ENABLE = 1
+} VnVpeEnable;
+
+/** \brief VPE Heading modes used by the VPE Basic Control register. */
+typedef enum
+{
+	/** \brief Absolute heading.
+	 */
+	VNHEADINGMODE_ABSOLUTE = 0,
+	/** \brief Relative heading.
+	 */
+	VNHEADINGMODE_RELATIVE = 1,
+	/** \brief Indoor heading.
+	 */
+	VNHEADINGMODE_INDOOR = 2
+} VnHeadingMode;
+
+/** \brief VPE modes for the VPE Basic Control register. */
+typedef enum
+{
+	/** \brief Off.
+	 */
+	VNVPEMODE_OFF = 0,
+	/** \brief Mode 1.
+	 */
+	VNVPEMODE_MODE1 = 1
+} VnVpeMode;
+
+/** \brief Different scenario modes for the INS Basic Configuration register. */
+typedef enum
+{
+	/** \brief AHRS.
+	 */
+	VNSCENARIO_AHRS = 0,
+	/** \brief General purpose INS with barometric pressure sensor.
+	 */
+	VNSCENARIO_INSWITHPRESSURE = 1,
+	/** \brief General purpose INS without barometric pressure sensor.
+	 */
+	VNSCENARIO_INSWITHOUTPRESSURE = 2,
+	/** \brief GPS moving baseline for dynamic applications.
+	 */
+	VNSCENARIO_GPSMOVINGBASELINEDYNAMIC = 3,
+	/** \brief GPS moving baseline for static applications.
+	 */
+	VNSCENARIO_GPSMOVINGBASELINESTATIC = 4
+} VnScenario;
+
+/** \brief HSI modes used for the Magnetometer Calibration Control register. */
+typedef enum
+{
+	/** \brief Real-time hard/soft iron calibration algorithm is turned off.
+	 */
+	VNHSIMODE_OFF = 0,
+	/** \brief Runs the real-time hard/soft iron calibration algorithm.
+	 */
+	VNHSIMODE_RUN = 1,
+	/** \brief Resets the real-time hard/soft iron solution.
+	 */
+	VNHSIMODE_RESET = 2
+} VnHsiMode;
+
+/** \brief HSI output types for the Magnetometer Calibration Control register. */
+typedef enum
+{
+	/** \brief Onboard HSI is not applied to the magnetic measurements.
+	 */
+	VNHSIOUTPUT_NOONBOARD = 1,
+	/** \brief Onboard HSI is applied to the magnetic measurements.
+	 */
+	VNHSIOUTPUT_USEONBOARD = 3
+} VnHsiOutput;
+
+/** \brief Type of velocity compensation performed by the VPE. */
+typedef enum
+{
+	/** \brief Disabled
+	 */
+	VNVELOCITYCOMPENSATIONMODE_DISABLED = 0,
+	/** \brief Body Measurement
+	 */
+	VNVELOCITYCOMPENSATIONMODE_BODYMEASUREMENT = 1
+} VnVelocityCompensationMode;
+
+/** \brief How the magnetometer is used by the filter. */
+typedef enum
+{
+	/** \brief Magnetometer will only affect heading.
+	 */
+	VNMAGNETICMODE_2D = 0,
+	/** \brief Magnetometer will affect heading, pitch, and roll.
+	 */
+	VNMAGNETICMODE_3D = 1
+} VnMagneticMode;
+
+/** \brief Source for magnetometer used by the filter. */
+typedef enum
+{
+	/** \brief Use internal magnetometer.
+	 */
+	VNEXTERNALSENSORMODE_INTERNAL = 0,
+	/** \brief Use external magnetometer. Will use measurement at every new time step.
+	 */
+	VNEXTERNALSENSORMODE_EXTERNAL200HZ = 1,
+	/** \brief Use external magnetometer. Will only use when the measurement is updated.
+	 */
+	VNEXTERNALSENSORMODE_EXTERNALONUPDATE = 2
+} VnExternalSensorMode;
+
+/** \brief Options for the use of FOAM. */
+typedef enum
+{
+	/** \brief FOAM is not used.
+	 */
+	VNFOAMINIT_NOFOAMINIT = 0,
+	/** \brief FOAM is used to initialize only pitch and roll.
+	 */
+	VNFOAMINIT_FOAMINITPITCHROLL = 1,
+	/** \brief FOAM is used to initialize heading, pitch and roll.
+	 */
+	VNFOAMINIT_FOAMINITHEADINGPITCHROLL = 2,
+	/** \brief FOAM is used to initialize pitch, roll and covariance.
+	 */
+	VNFOAMINIT_FOAMINITPITCHROLLCOVARIANCE = 3,
+	/** \brief FOAM is used to initialize heading, pitch, roll and covariance
+	 */
+	VNFOAMINIT_FOAMINITHEADINGPITCHROLLCOVARIANCE = 4
+} VnFoamInit;
+
+/** \brief The different types of processors. */
+typedef enum
+{
+	VNPROCESSOR_NAV,	/**< Navigation Processor. */
+	VNPROCESSOR_GPS,	/**< GPS Processor. */
+	VNPROCESSOR_IMU   /**< IMU Processor. */
+} VnProcessorType;
+
+#endif

src/drivers/ins/vectornav/libvnc/include/vn/error.h

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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/upackf.h

@@ -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/compositedata.h

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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/upackf.c

@@ -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/compositedata.c

@@ -0,0 +1,763 @@
+#include <math.h>
+#include <string.h>
+
+#include "vn/sensors/compositedata.h"
+#include "vn/xplat/criticalsection.h"
+#include "vn/protocol/upack.h"
+#include "vn/math/vector.h"
+#include "vn/math/matrix.h"
+
+float VnCompositeData_calculateCourseOverGround(float velNedX, float velNedY);
+float VnCompositeData_calculateSpeedOverGround(float velNedX, float velNedY);
+
+void VnCompositeData_initialize(VnCompositeData* compositeData)
+{
+	memset(compositeData, 0, sizeof(VnCompositeData));
+}
+
+bool VnCompositeData_hasCourseOverGround(VnCompositeData* compositeData)
+{
+	return (CDVEL_None != compositeData->velocityType &&
+		CDVEL_EstimatedBody != compositeData->velocityType &&
+		CDVEL_EstimatedEcef != compositeData->velocityType &&
+		CDVEL_GpsEcef != compositeData->velocityType);
+}
+
+bool VnCompositeData_courseOverGround(VnCompositeData* compositeData, float* courseOverGroundOut)
+{
+	bool success = false;
+
+	if (VnCompositeData_hasCourseOverGround(compositeData))
+	{
+		switch (compositeData->velocityType)
+		{
+		case CDVEL_GpsNed:
+			*courseOverGroundOut = VnCompositeData_calculateCourseOverGround(compositeData->velocityGpsNed.c[0],
+				compositeData->velocityGpsNed.c[1]);
+			success = true;
+			break;
+		case CDVEL_EstimatedNed:
+			*courseOverGroundOut = VnCompositeData_calculateCourseOverGround(compositeData->velocityGpsNed.c[0],
+				compositeData->velocityGpsNed.c[1]);
+			success = true;
+			break;
+		default:
+			break;
+		}
+	}
+
+	return success;
+}
+
+float VnCompositeData_calculateCourseOverGround(float velNedX, float velNedY)
+{
+	/* This is handled by calculating the atan2 of the input. */
+	/* Since the input for this is a velocity then we only need */
+	/* XY coordinates to calculate. */
+	return (float)atan2(velNedY, velNedX);
+}
+
+bool VnCompositeData_hasSpeedOverGround(VnCompositeData* compositeData)
+{
+	return (CDVEL_None != compositeData->velocityType &&
+		CDVEL_EstimatedBody != compositeData->velocityType &&
+		CDVEL_EstimatedEcef != compositeData->velocityType &&
+		CDVEL_GpsEcef != compositeData->velocityType);
+}
+
+bool VnCompositeData_speedOverGround(VnCompositeData* compositeData, float* speedOverGroundOut)
+{
+	bool success = false;
+
+	if (VnCompositeData_hasSpeedOverGround(compositeData))
+	{
+		switch (compositeData->velocityType)
+		{
+		case CDVEL_GpsNed:
+			*speedOverGroundOut = VnCompositeData_calculateSpeedOverGround(compositeData->velocityGpsNed.c[0],
+				compositeData->velocityGpsNed.c[1]);
+			success = true;
+			break;
+		case CDVEL_EstimatedNed:
+			*speedOverGroundOut = VnCompositeData_calculateSpeedOverGround(compositeData->velocityGpsNed.c[0],
+				compositeData->velocityGpsNed.c[1]);
+			success = true;
+			break;
+		default:
+			break;
+		}
+	}
+
+	return success;
+}
+
+float VnCompositeData_calculateSpeedOverGround(float velNedX, float velNedY)
+{
+	/* This is handled by calculating the magnitude of the input. */
+	/* Since the input for this is a velocity then we only need */
+	/* XY coordinates to calculate. */
+	return (float)sqrt((velNedX * velNedX) + (velNedY * velNedY));
+}
+
+void VnCompositeData_processBinaryPacket(VnCompositeData* compositeData, VnUartPacket* packet, VnCriticalSection* criticalSection)
+{
+	BinaryGroupType groups = (BinaryGroupType)VnUartPacket_groups(packet);
+	size_t curGroupFieldIndex = 0;
+
+	VnCriticalSection_enter(criticalSection);
+
+	if ((groups & BINARYGROUPTYPE_COMMON) != 0)
+		VnCompositeData_processBinaryPacketCommonGroup(compositeData, packet, (CommonGroup) VnUartPacket_groupField(packet, curGroupFieldIndex++));
+	if ((groups & BINARYGROUPTYPE_TIME) != 0)
+		VnCompositeData_processBinaryPacketTimeGroup(compositeData, packet, (TimeGroup) VnUartPacket_groupField(packet, curGroupFieldIndex++));
+	if ((groups & BINARYGROUPTYPE_IMU) != 0)
+		VnCompositeData_processBinaryPacketImuGroup(compositeData, packet, (ImuGroup)VnUartPacket_groupField(packet, curGroupFieldIndex++));
+	if ((groups & BINARYGROUPTYPE_GPS) != 0)
+		VnCompositeData_processBinaryPacketGpsGroup(compositeData, packet, (GpsGroup)VnUartPacket_groupField(packet, curGroupFieldIndex++));
+	if ((groups & BINARYGROUPTYPE_ATTITUDE) != 0)
+		VnCompositeData_processBinaryPacketAttitudeGroup(compositeData, packet, (AttitudeGroup)VnUartPacket_groupField(packet, curGroupFieldIndex++));
+	if ((groups & BINARYGROUPTYPE_INS) != 0)
+		VnCompositeData_processBinaryPacketInsGroup(compositeData, packet, (InsGroup)VnUartPacket_groupField(packet, curGroupFieldIndex++));
+  if ((groups & BINARYGROUPTYPE_GPS2) != 0)
+    VnCompositeData_processBinaryPacketGps2Group(compositeData, packet, (GpsGroup)VnUartPacket_groupField(packet, curGroupFieldIndex));
+
+	VnCriticalSection_leave(criticalSection);
+}
+
+void VnCompositeData_processBinaryPacketCommonGroup(VnCompositeData* compositeData, VnUartPacket* packet, CommonGroup commonGroup)
+{
+	if (commonGroup & COMMONGROUP_TIMESTARTUP)
+		compositeData->timeStartup = VnUartPacket_extractUint64(packet);
+
+	if (commonGroup & COMMONGROUP_TIMEGPS)
+		compositeData->timeGps = VnUartPacket_extractUint64(packet);
+
+	if (commonGroup & COMMONGROUP_TIMESYNCIN)
+		compositeData->timeSyncIn = VnUartPacket_extractUint64(packet);
+
+	if (commonGroup & COMMONGROUP_YAWPITCHROLL)
+		compositeData->yawPitchRoll = VnUartPacket_extractVec3f(packet);
+
+	if (commonGroup & COMMONGROUP_QUATERNION)
+		compositeData->quaternion = VnUartPacket_extractVec4f(packet);
+
+	if (commonGroup & COMMONGROUP_ANGULARRATE)
+		compositeData->angularRate = VnUartPacket_extractVec3f(packet);
+
+	if (commonGroup & COMMONGROUP_POSITION)
+		compositeData->positionEstimatedLla = VnUartPacket_extractVec3d(packet);
+
+	if (commonGroup & COMMONGROUP_VELOCITY)
+		compositeData->velocityEstimatedNed = VnUartPacket_extractVec3f(packet);
+
+	if (commonGroup & COMMONGROUP_ACCEL)
+		compositeData->acceleration = VnUartPacket_extractVec3f(packet);
+
+	if (commonGroup & COMMONGROUP_IMU)
+	{
+		compositeData->accelerationUncompensated = VnUartPacket_extractVec3f(packet);
+		compositeData->angularRateUncompensated = VnUartPacket_extractVec3f(packet);
+	}
+
+	if (commonGroup & COMMONGROUP_MAGPRES)
+	{
+		compositeData->magnetic = VnUartPacket_extractVec3f(packet);
+		compositeData->temperature = VnUartPacket_extractFloat(packet);
+		compositeData->pressure = VnUartPacket_extractFloat(packet);
+	}
+
+	if (commonGroup & COMMONGROUP_DELTATHETA)
+	{
+		compositeData->deltaTime = VnUartPacket_extractFloat(packet);
+		compositeData->deltaTheta = VnUartPacket_extractVec3f(packet);
+		compositeData->deltaVelocity = VnUartPacket_extractVec3f(packet);
+	}
+
+	if (commonGroup & COMMONGROUP_INSSTATUS)
+	{
+		/* Don't know if this is a VN-100, VN-200 or VN-300 so we can't know for sure if
+		this is VpeStatus or InsStatus. */
+		compositeData->vpeStatus = compositeData->insStatus = VnUartPacket_extractUint16(packet);
+	}
+
+	if (commonGroup & COMMONGROUP_SYNCINCNT)
+		compositeData->syncInCnt = VnUartPacket_extractUint32(packet);
+
+	if (commonGroup & COMMONGROUP_TIMEGPSPPS)
+		compositeData->timeGpsPps = VnUartPacket_extractUint64(packet);
+}
+
+void VnCompositeData_processBinaryPacketTimeGroup(VnCompositeData* compositeData, VnUartPacket* packet, TimeGroup timeGroup)
+{
+	if (timeGroup & TIMEGROUP_TIMESTARTUP)
+		compositeData->timeStartup = VnUartPacket_extractUint64(packet);
+
+	if (timeGroup & TIMEGROUP_TIMEGPS)
+		compositeData->timeGps = VnUartPacket_extractUint64(packet);
+
+	if (timeGroup & TIMEGROUP_GPSTOW)
+		compositeData->gpsTow = VnUartPacket_extractUint64(packet);
+
+	if (timeGroup & TIMEGROUP_GPSWEEK)
+		compositeData->week = VnUartPacket_extractUint16(packet);
+
+	if (timeGroup & TIMEGROUP_TIMESYNCIN)
+		compositeData->timeSyncIn = VnUartPacket_extractUint64(packet);
+
+	if (timeGroup & TIMEGROUP_TIMEGPSPPS)
+		compositeData->timeGpsPps = VnUartPacket_extractUint64(packet);
+
+	if (timeGroup & TIMEGROUP_TIMEUTC)
+	{
+		compositeData->timeUtc.year = VnUartPacket_extractInt8(packet);
+		compositeData->timeUtc.month = VnUartPacket_extractUint8(packet);
+		compositeData->timeUtc.day = VnUartPacket_extractUint8(packet);
+		compositeData->timeUtc.hour = VnUartPacket_extractUint8(packet);
+		compositeData->timeUtc.min = VnUartPacket_extractUint8(packet);
+		compositeData->timeUtc.sec = VnUartPacket_extractUint8(packet);
+		compositeData->timeUtc.ms = VnUartPacket_extractUint16(packet);
+	}
+
+	if (timeGroup & TIMEGROUP_SYNCINCNT)
+		compositeData->syncInCnt = VnUartPacket_extractUint32(packet);
+
+	if (timeGroup & TIMEGROUP_SYNCOUTCNT)
+		compositeData->syncOutCnt = VnUartPacket_extractUint32(packet);
+}
+
+void VnCompositeData_processBinaryPacketImuGroup(VnCompositeData* compositeData, VnUartPacket* packet, ImuGroup imuGroup)
+{
+	if (imuGroup & IMUGROUP_IMUSTATUS)
+		/* This field is currently reserved. */
+		VnUartPacket_extractUint16(packet);
+
+	if (imuGroup & IMUGROUP_UNCOMPMAG)
+		compositeData->magneticUncompensated = VnUartPacket_extractVec3f(packet);
+
+	if (imuGroup & IMUGROUP_UNCOMPACCEL)
+		compositeData->accelerationUncompensated = VnUartPacket_extractVec3f(packet);
+
+	if (imuGroup & IMUGROUP_UNCOMPGYRO)
+		compositeData->angularRateUncompensated = VnUartPacket_extractVec3f(packet);
+
+	if (imuGroup & IMUGROUP_TEMP)
+		compositeData->temperature = VnUartPacket_extractFloat(packet);
+
+	if (imuGroup & IMUGROUP_PRES)
+		compositeData->pressure = VnUartPacket_extractFloat(packet);
+
+	if (imuGroup & IMUGROUP_DELTATHETA)
+	{
+		compositeData->deltaTime = VnUartPacket_extractFloat(packet);
+		compositeData->deltaTheta = VnUartPacket_extractVec3f(packet);
+	}
+
+	if (imuGroup & IMUGROUP_DELTAVEL)
+		compositeData->deltaVelocity = VnUartPacket_extractVec3f(packet);
+
+	if (imuGroup & IMUGROUP_MAG)
+		compositeData->magnetic = VnUartPacket_extractVec3f(packet);
+
+	if (imuGroup & IMUGROUP_ACCEL)
+		compositeData->acceleration = VnUartPacket_extractVec3f(packet);
+
+	if (imuGroup & IMUGROUP_ANGULARRATE)
+		compositeData->angularRate = VnUartPacket_extractVec3f(packet);
+
+	if (imuGroup & IMUGROUP_SENSSAT)
+		compositeData->sensSat = VnUartPacket_extractUint16(packet);
+
+#ifdef EXTRA
+
+	if (imuGroup & IMUGROUP_RAW)
+	{
+		compositeData->magneticRaw = VnUartPacket_extractVec3f(packet);
+		compositeData->accelerationRaw = VnUartPacket_extractVec3f(packet);
+		compositeData->angularRateRaw = VnUartPacket_extractVec3f(packet);
+		compositeData->temperatureRaw = VnUartPacket_extractFloat(packet);
+	}
+
+#endif
+}
+
+void VnCompositeData_processBinaryPacketGpsGroup(VnCompositeData* compositeData, VnUartPacket* packet, GpsGroup gpsGroup)
+{
+	if (gpsGroup & GPSGROUP_UTC)
+	{
+		compositeData->timeUtc.year = VnUartPacket_extractInt8(packet);
+		compositeData->timeUtc.month = VnUartPacket_extractUint8(packet);
+		compositeData->timeUtc.day = VnUartPacket_extractUint8(packet);
+		compositeData->timeUtc.hour = VnUartPacket_extractUint8(packet);
+		compositeData->timeUtc.min = VnUartPacket_extractUint8(packet);
+		compositeData->timeUtc.sec = VnUartPacket_extractUint8(packet);
+		compositeData->timeUtc.ms = VnUartPacket_extractUint16(packet);
+	}
+
+	if (gpsGroup & GPSGROUP_TOW)
+		compositeData->gpsTow = VnUartPacket_extractUint64(packet);
+
+	if (gpsGroup & GPSGROUP_WEEK)
+		compositeData->week = VnUartPacket_extractUint16(packet);
+
+	if (gpsGroup & GPSGROUP_NUMSATS)
+		compositeData->numSats = VnUartPacket_extractUint8(packet);
+
+	if (gpsGroup & GPSGROUP_FIX)
+		compositeData->gpsFix = VnUartPacket_extractUint8(packet);
+
+	if (gpsGroup & GPSGROUP_POSLLA)
+		compositeData->positionGpsLla = VnUartPacket_extractVec3d(packet);
+
+	if (gpsGroup & GPSGROUP_POSECEF)
+		compositeData->positionGpsEcef = VnUartPacket_extractVec3d(packet);
+
+	if (gpsGroup & GPSGROUP_VELNED)
+		compositeData->velocityGpsNed = VnUartPacket_extractVec3f(packet);
+
+	if (gpsGroup & GPSGROUP_VELECEF)
+		compositeData->velocityGpsEcef = VnUartPacket_extractVec3f(packet);
+
+	if (gpsGroup & GPSGROUP_POSU)
+		compositeData->positionUncertaintyGpsNed = VnUartPacket_extractVec3f(packet);
+
+	if (gpsGroup & GPSGROUP_VELU)
+		compositeData->velocityUncertaintyGps = VnUartPacket_extractFloat(packet);
+
+	if (gpsGroup & GPSGROUP_TIMEU)
+		compositeData->timeUncertainty = VnUartPacket_extractUint32(packet);
+
+  if (gpsGroup & GPSGROUP_TIMEINFO)
+    compositeData->timeInfo = VnUartPacket_extractTimeInfo(packet);
+
+  if (gpsGroup & GPSGROUP_DOP)
+    compositeData->dop = VnUartPacket_extractGpsDop(packet);
+}
+
+void VnCompositeData_processBinaryPacketAttitudeGroup(VnCompositeData* compositeData, VnUartPacket* packet, AttitudeGroup attitudeGroup)
+{
+	if (attitudeGroup & ATTITUDEGROUP_VPESTATUS)
+		compositeData->vpeStatus = VnUartPacket_extractUint16(packet);
+
+	if (attitudeGroup & ATTITUDEGROUP_YAWPITCHROLL)
+		compositeData->yawPitchRoll = VnUartPacket_extractVec3f(packet);
+
+	if (attitudeGroup & ATTITUDEGROUP_QUATERNION)
+		compositeData->quaternion = VnUartPacket_extractVec4f(packet);
+
+	if (attitudeGroup & ATTITUDEGROUP_DCM)
+		compositeData->directionCosineMatrix = VnUartPacket_extractMat3f(packet);
+
+	if (attitudeGroup & ATTITUDEGROUP_MAGNED)
+		compositeData->magneticNed = VnUartPacket_extractVec3f(packet);
+
+	if (attitudeGroup & ATTITUDEGROUP_ACCELNED)
+		compositeData->accelerationNed = VnUartPacket_extractVec3f(packet);
+
+	if (attitudeGroup & ATTITUDEGROUP_LINEARACCELBODY)
+	{
+		compositeData->accelerationLinearBody = VnUartPacket_extractVec3f(packet);
+		compositeData->velocityType = CDVEL_EstimatedBody;
+	}
+
+	if (attitudeGroup & ATTITUDEGROUP_LINEARACCELNED)
+		compositeData->accelerationLinearNed = VnUartPacket_extractVec3f(packet);
+
+	if (attitudeGroup & ATTITUDEGROUP_YPRU)
+		compositeData->attitudeUncertainty = VnUartPacket_extractVec3f(packet);
+
+#ifdef EXTRA
+
+	if (attitudeGroup & ATTITUDEGROUP_YPRRATE)
+		compositeData->yprRates = VnUartPacket_extractVec3f(packet);
+
+	if (attitudeGroup & ATTITUDEGROUP_STATEAHRS)
+	{
+		/* Currently not doing anything with these values. */
+		size_t i;
+		for (i = 0; i < 7; i++)
+			VnUartPacket_extractFloat(packet);
+	}
+
+	if (attitudeGroup & ATTITUDEGROUP_COVAHRS)
+	{
+		/* Currently not doing anything with these values. */
+		size_t i;
+		for (i = 0; i < 6; i++)
+			VnUartPacket_extractFloat(packet);
+	}
+
+#endif
+}
+
+void VnCompositeData_processBinaryPacketInsGroup(VnCompositeData* compositeData, VnUartPacket* packet, InsGroup insGroup)
+{
+	if (insGroup & INSGROUP_INSSTATUS)
+		compositeData->insStatus = VnUartPacket_extractUint16(packet);
+
+	if (insGroup & INSGROUP_POSLLA)
+		compositeData->positionEstimatedLla = VnUartPacket_extractVec3d(packet);
+
+	if (insGroup & INSGROUP_POSECEF)
+		compositeData->positionEstimatedEcef = VnUartPacket_extractVec3d(packet);
+
+	if (insGroup & INSGROUP_VELBODY)
+	{
+		compositeData->velocityEstimatedBody = VnUartPacket_extractVec3f(packet);
+		compositeData->velocityType = CDVEL_EstimatedNed;
+	}
+
+	if (insGroup & INSGROUP_VELNED)
+		compositeData->velocityEstimatedNed = VnUartPacket_extractVec3f(packet);
+
+	if (insGroup & INSGROUP_VELECEF)
+		compositeData->velocityEstimatedEcef = VnUartPacket_extractVec3f(packet);
+
+	if (insGroup & INSGROUP_MAGECEF)
+		compositeData->magneticEcef = VnUartPacket_extractVec3f(packet);
+
+	if (insGroup & INSGROUP_ACCELECEF)
+		compositeData->accelerationEcef = VnUartPacket_extractVec3f(packet);
+
+	if (insGroup & INSGROUP_LINEARACCELECEF)
+		compositeData->accelerationLinearEcef = VnUartPacket_extractVec3f(packet);
+
+	if (insGroup & INSGROUP_POSU)
+		compositeData->positionUncertaintyEstimated = VnUartPacket_extractFloat(packet);
+
+	if (insGroup & INSGROUP_VELU)
+		compositeData->velocityUncertaintyEstimated = VnUartPacket_extractFloat(packet);
+
+#ifdef EXTRA
+
+	if (insGroup & INSGROUP_STATEINS)
+	{
+		/* Currently not doing anything with these values. */
+		size_t i;
+		for (i = 0; i < 17; i++)
+			VnUartPacket_extractFloat(packet);
+	}
+
+	if (insGroup & INSGROUP_COVINS)
+	{
+		/* Currently not doing anything with these values. */
+		size_t i;
+		for (i = 0; i < 16; i++)
+			VnUartPacket_extractFloat(packet);
+	}
+
+#endif
+}
+
+void VnCompositeData_processBinaryPacketGps2Group(VnCompositeData* compositeData, VnUartPacket* packet, GpsGroup gps2Group)
+{
+  if (gps2Group & GPSGROUP_UTC)
+  {
+    /* TODO: Need to store this in the current data. */
+    VnUartPacket_extractInt8(packet);
+    VnUartPacket_extractUint8(packet);
+    VnUartPacket_extractUint8(packet);
+    VnUartPacket_extractUint8(packet);
+    VnUartPacket_extractUint8(packet);
+    VnUartPacket_extractUint8(packet);
+    VnUartPacket_extractUint16(packet);
+  }
+
+  if (gps2Group & GPSGROUP_TOW)
+    compositeData->gps2Tow = VnUartPacket_extractUint64(packet);
+
+  if (gps2Group & GPSGROUP_WEEK)
+    compositeData->weekGps2 = VnUartPacket_extractUint16(packet);
+
+  if (gps2Group & GPSGROUP_NUMSATS)
+    compositeData->numSatsGps2 = VnUartPacket_extractUint8(packet);
+
+  if (gps2Group & GPSGROUP_FIX)
+    compositeData->fixGps2 = VnUartPacket_extractUint8(packet);
+
+  if (gps2Group & GPSGROUP_POSLLA)
+    compositeData->positionGps2Lla = VnUartPacket_extractVec3d(packet);
+
+  if (gps2Group & GPSGROUP_POSECEF)
+    compositeData->positionGps2Ecef = VnUartPacket_extractVec3d(packet);
+
+  if (gps2Group & GPSGROUP_VELNED)
+    compositeData->velocityGps2Ned = VnUartPacket_extractVec3f(packet);
+
+  if (gps2Group & GPSGROUP_VELECEF)
+    compositeData->velocityGps2Ecef = VnUartPacket_extractVec3f(packet);
+
+  if (gps2Group & GPSGROUP_POSU)
+    compositeData->positionUncertaintyGps2Ned = VnUartPacket_extractVec3f(packet);
+
+  if (gps2Group & GPSGROUP_VELU)
+    compositeData->velocityUncertaintyGps2 = VnUartPacket_extractFloat(packet);
+
+  if (gps2Group & GPSGROUP_TIMEU)
+    compositeData->timeUncertaintyGps2 = VnUartPacket_extractUint32(packet);
+
+#ifdef EXTRA
+
+  if (gps2Group & GPSGROUP_SVSTAT)
+  {
+    /* Current not doing anything with these values. */
+    size_t i;
+    for (i = 0; i < 8; i++)
+      VnUartPacket_extractFloat(packet);
+  }
+
+#endif
+}
+
+void VnCompositeData_processAsciiAsyncPacket(VnCompositeData* compositeData, VnUartPacket* packet, VnCriticalSection* criticalSection)
+{
+	VnCriticalSection_enter(criticalSection);
+
+	switch (VnUartPacket_determineAsciiAsyncType(packet))
+	{
+	case VNYPR:
+		VnUartPacket_parseVNYPR(packet, &compositeData->yawPitchRoll);
+		break;
+
+	case VNQTN:
+		VnUartPacket_parseVNQTN(packet, &compositeData->quaternion);
+		break;
+
+#ifdef EXTRA
+
+	case VNQTM:
+		VnUartPacket_parseVNQTM(packet, &compositeData->quaternion, &compositeData->magnetic);
+		break;
+
+	case VNQTA:
+		VnUartPacket_parseVNQTA(packet, &compositeData->quaternion, &compositeData->acceleration);
+		break;
+
+	case VNQTR:
+		VnUartPacket_parseVNQTR(packet, &compositeData->quaternion, &compositeData->angularRate);
+		break;
+
+	case VNQMA:
+		VnUartPacket_parseVNQMA(packet, &compositeData->quaternion, &compositeData->magnetic, &compositeData->acceleration);
+		break;
+
+	case VNQAR:
+		VnUartPacket_parseVNQAR(packet, &compositeData->quaternion, &compositeData->acceleration, &compositeData->angularRate);
+		break;
+
+#endif
+
+	case VNQMR:
+		VnUartPacket_parseVNQMR(packet, &compositeData->quaternion, &compositeData->magnetic, &compositeData->acceleration, &compositeData->angularRate);
+		break;
+
+#ifdef EXTRA
+
+	case VNDCM:
+		VnUartPacket_parseVNDCM(packet, &compositeData->directionCosineMatrix);
+		break;
+
+#endif
+
+	case VNMAG:
+		VnUartPacket_parseVNMAG(packet, &compositeData->magnetic);
+		break;
+
+	case VNACC:
+		VnUartPacket_parseVNACC(packet, &compositeData->acceleration);
+		break;
+
+	case VNGYR:
+		VnUartPacket_parseVNGYR(packet, &compositeData->angularRate);
+		break;
+
+	case VNMAR:
+		VnUartPacket_parseVNMAR(packet, &compositeData->magnetic, &compositeData->acceleration, &compositeData->angularRate);
+		break;
+
+	case VNYMR:
+		VnUartPacket_parseVNYMR(packet, &compositeData->yawPitchRoll, &compositeData->magnetic, &compositeData->acceleration, &compositeData->angularRate);
+		break;
+
+#ifdef EXTRA
+
+	case VNYCM:
+		VnUartPacket_parseVNYCM(
+			packet,
+			&compositeData->yawPitchRoll,
+			&compositeData->magnetic,
+			&compositeData->acceleration,
+			&compositeData->angularRateUncompensated,
+			&compositeData->temperature);
+		break;
+
+#endif
+
+	case VNYBA:
+		VnUartPacket_parseVNYBA(packet, &compositeData->yawPitchRoll, &compositeData->accelerationLinearBody, &compositeData->angularRate);
+		compositeData->velocityType = CDVEL_EstimatedBody;
+		break;
+
+	case VNYIA:
+		VnUartPacket_parseVNYIA(packet, &compositeData->yawPitchRoll, &compositeData->accelerationLinearNed, &compositeData->angularRate);
+		break;
+
+#ifdef EXTRA
+
+	case VNICM:
+		// TODO: Implement once figure out how to store/process the inertial mag/accel.
+		break;
+
+#endif
+
+	case VNIMU:
+		VnUartPacket_parseVNIMU(
+			packet,
+			&compositeData->magneticUncompensated,
+			&compositeData->accelerationUncompensated,
+			&compositeData->angularRateUncompensated,
+			&compositeData->temperature,
+			&compositeData->pressure);
+		break;
+
+	case VNGPS:
+	{
+		float timeUncertainty;
+		VnUartPacket_parseVNGPS(
+			packet,
+			&compositeData->tow,
+			&compositeData->week,
+			&compositeData->gpsFix,
+			&compositeData->numSats,
+			&compositeData->positionGpsLla,
+			&compositeData->velocityGpsNed,
+			&compositeData->positionUncertaintyGpsNed,
+			&compositeData->velocityUncertaintyGps,
+			&timeUncertainty);
+		/* Convert to UInt32 since this is the binary representation in nanoseconds. */
+		compositeData->timeUncertainty = (uint32_t)(timeUncertainty * 1e9);
+		compositeData->velocityType = CDVEL_GpsNed;
+		break;
+	}
+
+	case VNGPE:
+	{
+		float timeUncertainty;
+		VnUartPacket_parseVNGPE(
+			packet,
+			&compositeData->tow,
+			&compositeData->week,
+			&compositeData->gpsFix,
+			&compositeData->numSats,
+			&compositeData->positionGpsEcef,
+			&compositeData->velocityGpsEcef,
+			&compositeData->positionUncertaintyGpsEcef,
+			&compositeData->velocityUncertaintyGps,
+			&timeUncertainty);
+		/* Convert to UInt32 since this is the binary representation in nanoseconds. */
+		compositeData->timeUncertainty = (uint32_t)(timeUncertainty * 1e9);
+		compositeData->velocityType = CDVEL_GpsEcef;
+		break;
+	}
+
+	case VNINS:
+	{
+		float attUncertainty;
+		VnUartPacket_parseVNINS(
+			packet,
+			&compositeData->tow,
+			&compositeData->week,
+			&compositeData->insStatus,
+			&compositeData->yawPitchRoll,
+			&compositeData->positionEstimatedLla,
+			&compositeData->velocityEstimatedNed,
+			&attUncertainty,
+			&compositeData->positionUncertaintyEstimated,
+			&compositeData->velocityUncertaintyEstimated);
+		/* Binary data provides 3 components to yaw, pitch, roll uncertainty. */
+		compositeData->attitudeUncertainty.c[0] = compositeData->attitudeUncertainty.c[1] = compositeData->attitudeUncertainty.c[2] = attUncertainty;
+		compositeData->velocityType = CDVEL_EstimatedNed;
+		break;
+	}
+
+	case VNINE:
+	{
+		float attUncertainty;
+		VnUartPacket_parseVNINE(
+			packet,
+			&compositeData->tow,
+			&compositeData->week,
+			&compositeData->insStatus,
+			&compositeData->yawPitchRoll,
+			&compositeData->positionEstimatedEcef,
+			&compositeData->velocityEstimatedEcef,
+			&attUncertainty,
+			&compositeData->positionUncertaintyEstimated,
+			&compositeData->velocityUncertaintyEstimated);
+		/* Binary data provides 3 components to yaw, pitch, roll uncertainty. */
+		compositeData->attitudeUncertainty.c[0] = compositeData->attitudeUncertainty.c[1] = compositeData->attitudeUncertainty.c[2] = attUncertainty;
+		compositeData->velocityType = CDVEL_EstimatedEcef;
+		break;
+	}
+
+	case VNISL:
+		VnUartPacket_parseVNISL(
+			packet,
+			&compositeData->yawPitchRoll,
+			&compositeData->positionEstimatedLla,
+			&compositeData->velocityEstimatedNed,
+			&compositeData->acceleration,
+			&compositeData->angularRate);
+		compositeData->velocityType = CDVEL_EstimatedNed;
+		break;
+
+	case VNISE:
+		VnUartPacket_parseVNISE(
+			packet,
+			&compositeData->yawPitchRoll,
+			&compositeData->positionEstimatedEcef,
+			&compositeData->velocityEstimatedEcef,
+			&compositeData->acceleration,
+			&compositeData->angularRate);
+		compositeData->velocityType = CDVEL_EstimatedEcef;
+		break;
+
+	case VNDTV:
+		VnUartPacket_parseVNDTV(
+			packet,
+			&compositeData->deltaTime,
+			&compositeData->deltaTheta,
+			&compositeData->deltaVelocity);
+		break;
+
+#ifdef EXTRA
+
+	case VNRAW:
+		VnUartPacket_parseVNRAW(
+			packet,
+			&compositeData->magneticRaw,
+			&compositeData->accelerationRaw,
+			&compositeData->angularRateRaw,
+			&compositeData->temperatureRaw);
+		break;
+
+	case VNCMV:
+		/* TODO: Need to implement. */
+		break;
+
+	case VNSTV:
+		/* TODO: Need to implement. */
+		break;
+
+	case VNCOV:
+		/* TODO: Need to implement. */
+		break;
+
+#endif
+
+	default:
+		/* Not doing anything for unknown ASCII asynchronous packets. */
+		;
+	}
+
+	VnCriticalSection_leave(criticalSection);
+}

src/drivers/ins/vectornav/libvnc/src/vn/sensors/ezasyncdata.c

@@ -0,0 +1,94 @@
+#include "vn/sensors.h"
+#include "vn/sensors/compositedata.h"
+#include "vn/sensors/ezasyncdata.h"
+#include "vn/xplat/criticalsection.h"
+
+#include <math.h>
+#include <stdlib.h>
+#include <string.h>
+#include <vn/sensors/ezasyncdata.h>
+
+void VnEzAsyncData_processReceivedAsyncPacket(void *userData, VnUartPacket *packet, size_t runningIndex);
+
+VnError VnEzAsyncData_initializeAndConnect(VnEzAsyncData* ezAsyncData, const char* portName, uint32_t baudrate)
+{
+	VnError error;
+
+	/** \brief The associated connected sensor. */
+    ezAsyncData->sensor = (VnSensor*) malloc(sizeof(VnSensor));
+    ezAsyncData->curData = (VnCompositeData*) malloc(sizeof(VnCompositeData));
+    ezAsyncData->curDataCS = (VnCriticalSection*) malloc(sizeof(VnCompositeData));
+
+    if((ezAsyncData->sensor == NULL) ||
+       (ezAsyncData->curData == NULL) ||
+       (ezAsyncData->curDataCS == NULL))
+    {
+        error = E_MEMORY_NOT_ALLOCATED;
+        return error;
+    }
+
+	memset(ezAsyncData->curData, 0, sizeof(VnCompositeData));
+
+	if ((error = VnCriticalSection_initialize(ezAsyncData->curDataCS)) != E_NONE)
+		return error;
+
+	if ((error = VnSensor_initialize(ezAsyncData->sensor)) != E_NONE)
+		return error;
+
+	if ((error = VnSensor_connect(ezAsyncData->sensor, portName, baudrate)) != E_NONE)
+		return error;
+
+	ezAsyncData->curData->velocityType = CDVEL_None;
+
+	return VnSensor_registerAsyncPacketReceivedHandler(ezAsyncData->sensor, VnEzAsyncData_processReceivedAsyncPacket, ezAsyncData);
+}
+
+VnError VnEzAsyncData_disconnectAndUninitialize(VnEzAsyncData* ez)
+{
+  VnError error;
+
+  if ((error = VnSensor_unregisterAsyncPacketReceivedHandler(ez->sensor)) != E_NONE)
+    return error;
+
+  if ((error = VnCriticalSection_deinitialize(ez->curDataCS)) != E_NONE)
+    return error;
+
+  if ((error = VnSensor_disconnect(ez->sensor)) != E_NONE)
+    return error;
+
+  free(ez->curDataCS);
+  free(ez->curData);
+  free(ez->sensor);
+
+  return E_NONE;
+}
+
+VnCompositeData VnEzAsyncData_currentData(VnEzAsyncData* ez)
+{
+	VnCompositeData cd;
+
+	VnCriticalSection_enter(ez->curDataCS);
+
+	cd = *(ez->curData);
+
+	VnCriticalSection_leave(ez->curDataCS);
+
+	return cd;
+}
+
+VnSensor* VnEzAsyncData_sensor(VnEzAsyncData* ez)
+{
+	return ez->sensor;
+}
+
+void VnEzAsyncData_processReceivedAsyncPacket(void *userData, VnUartPacket *packet, size_t runningIndex)
+{
+	VnEzAsyncData* ez = (VnEzAsyncData*) userData;
+
+	(void)runningIndex;
+
+	if (VnUartPacket_isAsciiAsync(packet))
+        VnCompositeData_processAsciiAsyncPacket(ez->curData, packet, ez->curDataCS);
+	else
+        VnCompositeData_processBinaryPacket(ez->curData, packet, ez->curDataCS);
+}

src/drivers/ins/vectornav/libvnc/src/vn/sensors/searcher.c

@@ -0,0 +1,564 @@
+#include "vn/bool.h"
+#include "vn/sensors/searcher.h"
+#include "vn/xplat/serialport.h"
+#include "vn/xplat/thread.h"
+
+#include <ctype.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+
+
+#ifdef __linux__
+#include <errno.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#elif defined _WIN32
+#include <tchar.h>
+#endif
+
+uint32_t SubBD[9] = {9600, 19200, 38400, 57600, 115200, 128000, 230400, 460800, 921600};
+
+uint32_t VnSearcher_matchBaud(char inputData[])
+{
+
+	uint32_t inputBaud = 0;
+	char* start = strrchr(inputData, ',') + 1;
+	char* end =  strrchr(start, '*');
+	size_t index = 0;
+	bool integerDigit = true;
+
+	/* Isolate the baud in the message. */
+
+	/* Ensure the entire baud string was recieved. */
+	/* If end is null then this is an incomplete packet. */
+	if (NULL != end)
+	{
+		/* Ensure this is an integer digit. */
+		while ((start + index) < end)
+		{
+			char character = start[index];
+			if (0 == isdigit((int)character))
+			{
+				integerDigit = false;
+				break;
+			}
+			++index;
+		}
+
+		/* Convert the string to an integer for comparing. */
+		if (integerDigit)
+		{
+			char stringBaud[20] = { 0 };
+			uint32_t length = end - start;
+
+			memcpy(stringBaud, start, length);
+			inputBaud = atoi(stringBaud);
+		}
+	}
+
+	/* Return the found baud */
+	return inputBaud;
+}
+
+void VnSearcher_discardData(char inputData[], size_t dataLength, size_t discardLength)
+{
+	/* This function will move the pertinent data to be begining of the array. */
+	/* If discard length is the full size of the array then all data is discarded. */
+	if (dataLength <= discardLength)
+	{
+		/* Just zero out everything */
+		memset(inputData, 0, dataLength);
+	}
+	else
+	{
+		/* Move the data to the front of the array and zero out everything else */
+		/* Size of the data to move */
+		size_t tmpSize = dataLength - discardLength;
+		/* Temporary swap buffer */
+		char* tmp = (char*)malloc(sizeof(char) * tmpSize);
+		/* And the swap operations */
+		/*memcpy_s(tmp, tmpSize, inputData + discardLength, tmpSize);*/
+		memcpy(tmp, inputData + discardLength, tmpSize);
+		memset(inputData, 0, dataLength);
+		/*memcpy_s(inputData, tmpSize, tmp, tmpSize);*/
+		memcpy(inputData, tmp, tmpSize);
+	}
+}
+
+bool VnSearcher_findString(char* inputData, size_t* inputSize, char* toFind, size_t findSize)
+{
+	/* Assume failure */
+	bool success = false;
+	char* end = inputData + *inputSize;
+	char* start = inputData;
+	size_t size = *inputSize;
+
+	/* Loop through the data until we run out */
+	while ((NULL != start) && (!success) && (findSize <= size) && (NULL != end))
+	{
+		/* find the first instance of the toFind array */
+		start = (char*)memchr(start, '$', size);
+
+		/* It doesn't exist so just zero out everything; the data is useless */
+		if (NULL == start)
+		{
+			VnSearcher_discardData(inputData, *inputSize, *inputSize);
+			*inputSize = 0;
+		}
+		/* We found it, now to see if this is the beginning of the input array */
+		else
+		{
+			end = (char*)memchr(start, '*', size);
+
+			/* We found an end sentinal */
+			if (NULL != end)
+			{
+				/* We have both a start and end sentinal.  Let's see if */
+				/* we have a proper packet. */
+
+				if(((end - start) >= (int)findSize) &&
+				   (strncmp(start, toFind, findSize) == 0))
+				{
+					/* This is what we're looking for */
+					success = true;
+
+					/* Readjust the size of the data. */
+					VnSearcher_discardData(inputData, *inputSize, start - inputData);
+					*inputSize = end - start + 1;
+				}
+				else
+				{
+					/* Wrong type of packet or no packet.  Remove the sentinal character */
+					/* and continue the search. */
+					start[0] = '\0';
+				}
+			}
+
+		}
+	}
+
+	return success;
+}
+
+bool VnSearcher_validateData(char inputData[], size_t* dataLength)
+{
+	/* Assume failure */
+	bool validated = false;
+	bool result1 = false;
+	bool result2 = false;
+
+	char search1[256] = { 0 };
+	char search2[256] = { 0 };
+
+	size_t size1 = *dataLength;
+	size_t size2 = *dataLength;
+
+	memcpy(search1, inputData, *dataLength);
+	memcpy(search2, inputData, *dataLength);
+
+	/* search for a $VNRRG,5 or %VNRRG,05 */
+
+	result1 = VnSearcher_findString(search1, &size1, "$VNRRG,5", 8);
+	result2 = VnSearcher_findString(search2, &size2, "$VNRRG,05", 9);
+
+	/*printf("validateData::search1-1 %s\n", search1);*/
+	if (result1)
+	{
+		validated = true;
+		VnSearcher_discardData(inputData, *dataLength, *dataLength);
+		memcpy(inputData, search1, size1);
+		*dataLength = size1;
+	}
+	else if (result2)
+	{
+		validated = true;
+		VnSearcher_discardData(inputData, *dataLength, *dataLength);
+		memcpy(inputData, search2, size2);
+		*dataLength = size2;
+	}
+	else if (size1 > size2)
+	{
+		*dataLength = size1;
+	}
+	else
+	{
+		*dataLength = size2;
+	}
+
+	/* Return the result. */
+	return validated;
+}
+
+void VnSearcher_dataReceivedHandler(void* portInfo)
+{
+	/* Cast the input void pointer */
+	VnPortInfo* port = (VnPortInfo*)portInfo;
+
+	/* Prepare for handling incoming data */
+	char readBuffer[255] = {0};
+
+	/* Data validation flag.  Assume failure. */
+	bool validated = false;
+
+	size_t bytesToRead = sizeof(port->data) - port->dataSize;
+	size_t bytesRead = 0;
+
+	/* Keep looping until a valid packet is found or the thread shuts down. */
+	/* Read the data from the port. */
+	VnError error = VnSerialPort_read(port->port, readBuffer, bytesToRead, &bytesRead);
+
+	/*printf("read %d %s\n", bytesRead, readBuffer);*/
+
+	memcpy(port->data + port->dataSize, readBuffer, bytesRead);
+	port->dataSize += bytesRead;
+	port->data[254] = '\n';
+
+
+	/* Check if this is valid data from the sensor. */
+	/*validated = VnSearcher_validateData(readBuffer);*/
+	validated = VnSearcher_validateData(port->data, &(port->dataSize));
+
+	/* If an error occurs place a value of -2 into the baud variable to
+		* indicate this and end the thread.
+		*/
+	if (E_NONE != error)
+	{
+		port->baud = -2;
+		/*break;*/
+	}
+
+	/* If this is valid data then extract the baud rate and put the value into
+	 * the port object.
+	 */
+	if(validated)
+	{
+		uint32_t bufferBaud = VnSearcher_matchBaud(port->data);
+		port->baud = bufferBaud;
+		validated = false;
+	}
+}
+
+void VnSearcher_testPort(void* portInfo)
+{
+	VnError error = E_NONE;
+	/* Cast the input void pointer */
+	VnPortInfo* port = (VnPortInfo*)portInfo;
+
+	size_t index = 0;
+	size_t numBaudrates = sizeof(SubBD) / sizeof(SubBD[0]);
+
+	/* Loop through all of the possible baud rates. */
+	while(index < numBaudrates)
+	{
+		/* Set up a serial port object to communicate with. */
+		VnSerialPort serialPort;
+		port->port = &serialPort;
+		VnSerialPort_initialize(&serialPort);
+		VnSerialPort_registerDataReceivedHandler(&serialPort, VnSearcher_dataReceivedHandler, port);
+
+		/* Open the port for communications. */
+		error = VnSerialPort_open(&serialPort, port->portName, SubBD[index]);
+
+		/* ensure the port opened. */
+		if(VnSerialPort_isOpen(&serialPort) && (E_NONE == error))
+		{
+			/* Request the baud rate from the sensor. */
+			VnSerialPort_write(&serialPort, "$VNRRG,05*XX\r\n", 14);
+
+			/* Now wait 50 miliseconds to see if the sensor answers. */
+			VnThread_sleepMs(50);
+
+			/* Close the port to tell the data handling thread to terminate. */
+			VnSerialPort_close(&serialPort);
+
+			/* Check if we have a baud.
+			 * -2 - Unknown error occured.
+			 * -1 - No baud rate was found.
+			 *  0 - Initial state.
+			 * >0 - A valid baud rate was found. */
+			if(port->baud != 0)
+			{
+				/* Either a sensor was found or an error occurred.
+				 * End the while loop. */
+				break;
+			}
+		}
+
+		/* We don't want an infinite loop. */
+		index++;
+	}
+
+	/* If there was no change in the baud variable indicate no sensor found
+	 * using a value of -1. */
+	if(port->baud == 0)
+	{
+		port->baud = -1;
+	}
+}
+
+/* Public Functions */
+
+char* VnSearcher_getPortName(VnPortInfo* portInfo)
+{
+	return portInfo->portName;
+}
+
+int32_t VnSearcher_getPortBaud(VnPortInfo* portInfo)
+{
+	return portInfo->baud;
+}
+
+#ifdef __linux__
+void VnSearcher_findPorts_LINUX(char*** portNamesOut, int32_t* numPortsFound)
+{
+	char portName[15] = {0};
+	char portNames[16 * 255] = {0};
+	char* portNamePtr = portNames;
+	const size_t MAX_PORTS = 255;
+	int portFd = -1;
+	size_t index = 0;
+	size_t nameLen = 0;
+
+	*numPortsFound = 0;
+
+	while(index < MAX_PORTS)
+	{
+		/* Create the port name for reading. */
+		sprintf(portName, "/dev/ttyUSB%u", (unsigned int) index);
+
+		/* Attempt to open the serial port */
+		portFd = open(portName,
+					  #if __linux__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+					  O_RDWR | O_NOCTTY );
+					  #elif __APPLE__
+					  O_RDWR | O_NOCTTY | O_NONBLOCK);
+					  #else
+					  #error "Unknown System"
+					  #endif
+
+		/* Check to see if the port opened */
+		if(portFd != -1)
+		{
+			/* We have a usable port */
+			/* Record the port name */
+			nameLen = strlen(portName);
+			sprintf(portNamePtr, "%s,", portName);
+			portNamePtr += nameLen + 1;
+			++(*numPortsFound);
+
+			/* Close the port */
+			close(portFd);
+		}
+		/* If not we ignore it */
+		++index;
+	}
+
+	if (*numPortsFound > 0)
+	{
+		char* start = NULL;
+		char* end = NULL;
+
+		uint32_t length = 0;
+
+		*portNamesOut = (char**)malloc(index * sizeof(char*));
+		index = 0;
+
+		start = portNames;
+		end = portNames;
+
+		while (index < *numPortsFound)
+		{
+			start = strstr(end, "/dev");
+
+			end = strchr(start, ',');
+			length = end - start;
+			(*portNamesOut)[index] = (char*)malloc(length + 1);
+			memcpy((*portNamesOut)[index], start, length);
+			(*portNamesOut)[index][length] = '\0';
+			end += 1;
+
+			++index;
+		}
+	}}
+#elif defined _WIN32
+void VnSearcher_findPorts_WIN32(char*** portNamesOut, int32_t* numPortsFound)
+{
+	uint32_t MAX_PORTS = 255;
+
+	char portName[12] = {0};
+	char portNames[13 * 255] = {0};
+	char* portNamePtr = portNames;
+
+	HANDLE portHandle;
+
+	uint32_t index = 0;
+	uint32_t portNameLength = 0;
+
+	#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
+
+	while (index < MAX_PORTS)
+	{
+		/* Create the port name for reading. */
+		sprintf(portName, "\\\\.\\COM%u", index);
+
+		/* Create the port file. */
+		portHandle = CreateFile(portName, GENERIC_READ | GENERIC_WRITE, 0, 0, OPEN_EXISTING, 0, 0);
+
+		/* Check if this is a valid handle. */
+		if (INVALID_HANDLE_VALUE != portHandle)
+		{
+			/* We have a valid handle. */
+			/* Record the port name and increment the number of ports counter. */
+			portNameLength = strlen(portName);
+			sprintf(portNamePtr, "%s,", portName);
+			portNamePtr += portNameLength + 1;
+			++(*numPortsFound);
+
+			/* Clean up.  Close the handle. */
+			if(!CloseHandle(portHandle))
+			{
+				printf("ERROR\n");
+			}
+
+
+		}
+
+		index++;
+	}
+
+	#if defined(_MSC_VER)
+	#pragma warning(pop)
+	#endif
+
+	if (*numPortsFound > 0)
+	{
+		char* start = portNames;
+		char* end = portNames;
+
+		*portNamesOut = (char**) malloc(index * sizeof(char*));
+		index = 0;
+
+		while ((int32_t)index < *numPortsFound)
+		{
+			uint32_t length;
+
+			start = strstr(end, "COM");
+
+			end = strchr(start, ',');
+			length = end - start;
+			(*portNamesOut)[index] = (char*)malloc(length + 1);
+			memcpy((*portNamesOut)[index], start, length);
+			(*portNamesOut)[index][length] = '\0';
+			end += 1;
+
+			index++;
+		}
+	}
+}
+#endif
+
+void VnSearcher_findPortBaud(char* portName, int32_t* foundBaudrate)
+{
+	/* These will handle the return data. */
+	VnPortInfo** portInfo;
+
+	/* These will handle the input data. */
+	char** portNames = &portName;
+	int32_t numPorts = 1;
+
+	/* This function will search the port and, if found, return a VnPortInfo */
+	/* containing the baud rate. */
+	VnSearcher_searchInputPorts(&portNames, numPorts, &portInfo);
+
+	/* The number in baud will indicate whether the function succeeded
+	 * -2 - error
+	 * -1 - no sensor found
+	 * >0 - sensor's baud rate
+	 */
+	*foundBaudrate = portInfo[0]->baud;
+}
+
+void VnSearcher_findAllSensors(VnPortInfo*** returnSensors, int32_t* numSensors)
+{
+	int32_t numSystemPorts = 0;
+	char** systemPorts = NULL;
+
+	/* First, find all of the ports on the system */
+	VnSearcher_findPorts(&systemPorts, &numSystemPorts);
+
+	/* Check if a sensor is accessible on the port */
+	/* Only do this if the port search succeeded and found available system ports */
+	if(numSystemPorts > 0)
+	{
+		VnSearcher_searchInputPorts(&systemPorts, numSystemPorts, returnSensors);
+		*numSensors = numSystemPorts;
+	}
+}
+
+void VnSearcher_searchInputPorts(char*** portsToCheck, int32_t numPortsToCheck, VnPortInfo*** sensorsFound)
+{
+	int32_t linkCount = 0;
+
+	/* Initialize the sensorsFound array with space for any future pointers. */
+	*sensorsFound = (VnPortInfo**) malloc(sizeof(VnPortInfo*) * numPortsToCheck);
+
+	while(linkCount < numPortsToCheck)
+	{
+        /*VnPortInfo* newPort = NULL;*/
+        size_t nameSize = 0;
+
+        /*newPort = (*sensorsFound)[linkCount];*/
+		nameSize = strlen((*portsToCheck)[linkCount]);
+
+		/* Create the port info */
+		(*sensorsFound)[linkCount] = (VnPortInfo*)malloc(sizeof(VnPortInfo));
+		(*sensorsFound)[linkCount]->dataSize = 0;
+
+		/* Give it the port name */
+		(*sensorsFound)[linkCount]->portName = (char*)malloc(sizeof(char) * nameSize + 1);
+		memcpy((*sensorsFound)[linkCount]->portName, (*portsToCheck)[linkCount], nameSize + 1);
+
+		/* Make sure the data field is zeroed out. */
+		memset(&(*sensorsFound)[linkCount]->data, 0, sizeof((*sensorsFound)[linkCount]->data));
+
+		/* Set baud to 0.  A baud of 0 will indicate that the test is not finished
+		 * running just yet.  A baud of -1 will indicate that no sensor has been
+		 * found.  A baud of -2 will indicate an error.  Any baud over 0 will be
+		 * the actual baud rate. */
+		(*sensorsFound)[linkCount]->baud = 0;
+
+		/* Set up the thread */
+
+		/* Now, start the thread to check the port.*/
+		VnThread_startNew(&((*sensorsFound)[linkCount]->thread), VnSearcher_testPort, (*sensorsFound)[linkCount]);
+
+		/* And increment the link counter */
+		++linkCount;
+	}
+
+	/* Reset the link counter. */
+	linkCount = 0;
+
+	/* Loop until all of the threads have completed. */
+	while(linkCount < numPortsToCheck)
+	{
+		/* Sleep until the baud rate changes */
+		while((*sensorsFound)[linkCount]->baud == 0)
+		{
+			VnThread_sleepMs(1);
+		}
+
+		/* The thread has finished.  The thread handles setting any pertinent
+		 * data.  Simply increment the linkCount variable
+		 */
+		++linkCount;
+	}
+}

src/drivers/ins/vectornav/libvnc/src/vn/util.c

@@ -0,0 +1,391 @@
+#include "vn/util.h"
+
+#include <string.h>
+#include <stdio.h>
+
+#if (defined _M_IX86 || defined __i386__ || defined __x86_64)
+	/* Compiling for x86 processor. */
+	#define VN_LITTLE_ENDIAN	1
+#elif defined __linux__ || defined __NUTTX__
+	/* Don't know what processor we are compiling for but we have endian.h. */
+	#define VN_HAVE_ENDIAN_H	1
+	#include <endian.h>
+#elif defined __ORDER_LITTLE_ENDIAN__
+	#define VN_LITTLE_ENDIAN	1
+#elif defined __ORDER_BIG_ENDIAN__
+	#define VN_BIG_ENDIAN	1
+#else
+	#error "Unknown System"
+#endif
+
+int VnApi_major(void)
+{
+	return VNAPI_MAJOR;
+}
+
+int VnApi_minor(void)
+{
+	return VNAPI_MINOR;
+}
+
+int VnApi_patch(void)
+{
+	return VNAPI_PATCH;
+}
+
+int VnApi_revision(void)
+{
+	return VNAPI_REVISION;
+}
+
+VnError VnApi_getVersion(char *out, size_t outLength)
+{
+	#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
+
+	outLength = sprintf(out, "%d.%d.%d.%d", VNAPI_MAJOR, VNAPI_MINOR, VNAPI_PATCH, VNAPI_REVISION);
+
+	#if defined(_MSC_VER)
+		#pragma warning(pop)
+	#endif
+
+	return E_NONE;
+}
+
+uint8_t VnUtil_toUint8FromHexChar(char c)
+{
+	if (c < ':')
+		return c - '0';
+
+	if (c < 'G')
+		return c - '7';
+
+	return c - 'W';
+}
+
+uint8_t VnUtil_toUint8FromHexStr(char const *str)
+{
+	uint8_t result;
+
+	result = VnUtil_toUint8FromHexChar(str[0]) << 4;
+	result += VnUtil_toUint8FromHexChar(str[1]);
+
+	return result;
+}
+
+uint16_t VnUtil_toUint16FromHexStr(char const *str)
+{
+	uint16_t result;
+
+	result = VnUtil_toUint8FromHexChar(str[0]) << 12;
+	result += VnUtil_toUint8FromHexChar(str[1]) << 8;
+	result += VnUtil_toUint8FromHexChar(str[2]) << 4;
+	result += VnUtil_toUint8FromHexChar(str[3]);
+
+	return result;
+}
+
+void VnUtil_toHexStrFromUint8(uint8_t toConvert, char* output)
+{
+	char const digits[] = "0123456789ABCDEF";
+
+	*output++ = digits[(toConvert / 16) % 16];
+	*output = digits[toConvert % 16];
+}
+
+void VnUtil_toHexStrFromUint16(uint16_t toConvert, char* output)
+{
+	char const digits[] = "0123456789ABCDEF";
+
+	*output++ = digits[(toConvert / (16*16*16)) % 16];
+	*output++ = digits[(toConvert / (16 * 16)) % 16];
+	*output++ = digits[(toConvert / 16) % 16];
+	*output = digits[toConvert % 16];
+}
+
+size_t VnUtil_toStrFromUint16(uint16_t toConvert, char *output)
+{
+	char const digits[] = "0123456789";
+
+	uint16_t shifter = toConvert;
+	size_t numOfChars = 0;
+	char* p = output;
+
+	/* Move to the representation end. */
+	do
+	{
+		++p;
+		shifter /= 10;
+		numOfChars++;
+	} while (shifter);
+
+	/* Move back to the beginning inserting our converted digits as we go. */
+	do
+	{
+		*--p = digits[toConvert % 10];
+		toConvert /= 10;
+	} while (toConvert);
+
+	return numOfChars;
+}
+
+uint8_t VnUtil_countSetBitsUint8(uint8_t data)
+{
+	uint8_t count = 0;
+
+	while (data)
+	{
+		data &= (data - 1);
+
+		count++;
+	}
+
+	return count;
+}
+
+void strFromBool(char *out, bool 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
+
+	if (val == 0)
+		strcpy(out, "false");
+	else
+		strcpy(out, "true");
+
+	#if defined(_MSC_VER)
+		#pragma warning(pop)
+	#endif
+}
+
+uint16_t stoh16(uint16_t sensorOrdered)
+{
+	#if VN_LITTLE_ENDIAN
+	return sensorOrdered;
+	#elif VN_BIG_ENDIAN
+	uint16_t host;
+	host = ((sensorOrdered >> 0) & 0xFF) * 0x0100;
+	host += ((sensorOrdered >> 8) & 0xFF) * 0x0001;
+	return host;
+	#elif VN_HAVE_ENDIAN_H
+	return le16toh(sensorOrdered);
+	#else
+	#error("Unknown system")
+	#endif
+}
+
+uint32_t stoh32(uint32_t sensorOrdered)
+{
+	#if VN_LITTLE_ENDIAN
+	return sensorOrdered;
+	#elif VN_BIG_ENDIAN
+	uint32_t host;
+	host = ((sensorOrdered >> 0) & 0xFF) * 0x01000000;
+	host += ((sensorOrdered >> 8) & 0xFF) * 0x00010000;
+	host += ((sensorOrdered >> 16) & 0xFF) * 0x00000100;
+	host += ((sensorOrdered >> 24) & 0xFF) * 0x00000001;
+	return host;
+	#elif VN_HAVE_ENDIAN_H
+	return le32toh(sensorOrdered);
+	#else
+	#error("Unknown system")
+	#endif
+}
+
+uint64_t stoh64(uint64_t sensorOrdered)
+{
+	#if VN_LITTLE_ENDIAN
+	return sensorOrdered;
+	#elif VN_BIG_ENDIAN
+	uint64_t host;
+	host = ((sensorOrdered >> 0) & 0xFF) * 0x0100000000000000;
+	host += ((sensorOrdered >> 8) & 0xFF) * 0x0001000000000000;
+	host += ((sensorOrdered >> 16) & 0xFF) * 0x0000010000000000;
+	host += ((sensorOrdered >> 24) & 0xFF) * 0x0000000100000000;
+	host += ((sensorOrdered >> 32) & 0xFF) * 0x0000000001000000;
+	host += ((sensorOrdered >> 40) & 0xFF) * 0x0000000000010000;
+	host += ((sensorOrdered >> 48) & 0xFF) * 0x0000000000000100;
+	host += ((sensorOrdered >> 56) & 0xFF) * 0x0000000000000001;
+	return host;
+	#elif VN_HAVE_ENDIAN_H
+	return le64toh(sensorOrdered);
+	#else
+	#error("Unknown system")
+	#endif
+}
+
+uint16_t htos16(uint16_t hostOrdered)
+{
+	return stoh16(hostOrdered);
+}
+
+uint32_t htos32(uint32_t hostOrdered)
+{
+	return stoh32(hostOrdered);
+}
+
+uint64_t htos64(uint64_t hostOrdered)
+{
+	return stoh64(hostOrdered);
+}
+
+float htosf4(float hostOrdered)
+{
+	uint32_t* v = (uint32_t*) &hostOrdered;
+
+	*v = stoh32(*v);
+
+	return hostOrdered;
+}
+
+double htosf8(double hostOrdered)
+{
+	uint64_t* v = (uint64_t*) &hostOrdered;
+
+	*v = stoh64(*v);
+
+	return hostOrdered;
+}
+
+uint16_t VnUtil_extractUint16(const char* pos)
+{
+	uint16_t d;
+
+	memcpy(&d, pos, sizeof(uint16_t));
+
+	return stoh16(d);
+}
+
+uint32_t VnUtil_extractUint32(const char* pos)
+{
+	uint32_t d;
+
+	memcpy(&d, pos, sizeof(uint32_t));
+
+	return stoh32(d);
+}
+
+float VnUtil_extractFloat(const char* pos)
+{
+	float f;
+	uint32_t tmp;
+
+	memcpy(&tmp, pos, sizeof(float));
+	tmp = stoh32(tmp);
+	memcpy(&f, &tmp, sizeof(float));
+
+	return f;
+}
+
+double VnUtil_extractDouble(const char* pos)
+{
+	double f;
+	uint64_t tmp;
+
+	memcpy(&tmp, pos, sizeof(double));
+	tmp = stoh64(tmp);
+	memcpy(&f, &tmp, sizeof(double));
+
+	return f;
+}
+
+vec3f VnUtil_extractVec3f(const char* pos)
+{
+	vec3f r;
+
+	r.c[0] = VnUtil_extractFloat(pos);
+	r.c[1] = VnUtil_extractFloat(pos + sizeof(float));
+	r.c[2] = VnUtil_extractFloat(pos + 2 * sizeof(float));
+
+	return r;
+}
+
+vec4f VnUtil_extractVec4f(const char* pos)
+{
+	vec4f r;
+
+	r.c[0] = VnUtil_extractFloat(pos);
+	r.c[1] = VnUtil_extractFloat(pos + sizeof(float));
+	r.c[2] = VnUtil_extractFloat(pos + 2 * sizeof(float));
+	r.c[3] = VnUtil_extractFloat(pos + 3 * sizeof(float));
+
+	return r;
+}
+
+vec3d VnUtil_extractVec3d(const char* pos)
+{
+	vec3d r;
+
+	r.c[0] = VnUtil_extractDouble(pos);
+	r.c[1] = VnUtil_extractDouble(pos + sizeof(double));
+	r.c[2] = VnUtil_extractDouble(pos + 2 * sizeof(double));
+
+	return r;
+}
+
+mat3f VnUtil_extractMat3f(const char* pos)
+{
+	mat3f r;
+
+	r.e[0] = VnUtil_extractFloat(pos);
+	r.e[1] = VnUtil_extractFloat(pos + sizeof(float));
+	r.e[2] = VnUtil_extractFloat(pos + 2 * sizeof(float));
+	r.e[3] = VnUtil_extractFloat(pos + 3 * sizeof(float));
+	r.e[4] = VnUtil_extractFloat(pos + 4 * sizeof(float));
+	r.e[5] = VnUtil_extractFloat(pos + 5 * sizeof(float));
+	r.e[6] = VnUtil_extractFloat(pos + 6 * sizeof(float));
+	r.e[7] = VnUtil_extractFloat(pos + 7 * sizeof(float));
+	r.e[8] = VnUtil_extractFloat(pos + 8 * sizeof(float));
+
+	return r;
+}
+
+GpsDop VnUtil_extractGpsDop(const char* pos)
+{
+  GpsDop dop;
+
+  dop.gDOP = VnUtil_extractFloat(pos);
+  dop.pDOP = VnUtil_extractFloat(pos + sizeof(float));
+  dop.tDOP = VnUtil_extractFloat(pos + 2 * sizeof(float));
+  dop.vDOP = VnUtil_extractFloat(pos + 3 * sizeof(float));
+  dop.hDOP = VnUtil_extractFloat(pos + 4 * sizeof(float));
+  dop.nDOP = VnUtil_extractFloat(pos + 5 * sizeof(float));
+  dop.eDOP = VnUtil_extractFloat(pos + 6 * sizeof(float));
+
+  return dop;
+}
+
+TimeUtc VnUtil_extractTimeUtc(const char* pos)
+{
+  TimeUtc timeUtc;
+
+  timeUtc.year  = (int8_t)pos[0];
+  timeUtc.month = (uint8_t)pos[1];
+  timeUtc.day   = (uint8_t)pos[2];
+  timeUtc.hour  = (uint8_t)pos[3];
+  timeUtc.min   = (uint8_t)pos[4];
+  timeUtc.sec   = (uint8_t)pos[5];
+  timeUtc.ms = VnUtil_extractUint16(pos + 6);
+
+  return timeUtc;
+}
+
+TimeInfo VnUtil_extractTimeInfo(const char* pos)
+{
+  TimeInfo timeInfo;
+
+  timeInfo.timeStatus = (uint8_t)pos[0];
+  timeInfo.leapSeconds = (int8_t)pos[1];
+
+  return timeInfo;
+}

src/drivers/ins/vectornav/libvnc/src/vn/xplat/criticalsection.c

@@ -0,0 +1,73 @@
+#include "vn/xplat/criticalsection.h"
+
+VnError VnCriticalSection_initialize(VnCriticalSection *cs)
+{
+	#if _WIN32
+
+	InitializeCriticalSection(&cs->handle);
+
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+
+	if (pthread_mutex_init(&cs->handle, NULL))
+		return E_UNKNOWN;
+
+	#else
+	#error "Unknown System"
+	#endif
+
+	return E_NONE;
+}
+
+VnError VnCriticalSection_deinitialize(VnCriticalSection *cs)
+{
+	#if _WIN32
+
+	DeleteCriticalSection(&cs->handle);
+
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+
+	if (pthread_mutex_destroy(&cs->handle))
+		return E_UNKNOWN;
+
+	#else
+	#error "Unknown System"
+	#endif
+
+	return E_NONE;
+}
+
+VnError VnCriticalSection_enter(VnCriticalSection *cs)
+{
+	#if _WIN32
+
+	EnterCriticalSection(&cs->handle);
+
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+
+	if (pthread_mutex_lock(&cs->handle))
+		return E_UNKNOWN;
+
+	#else
+	#error "Unknown System"
+	#endif
+
+	return E_NONE;
+}
+
+VnError VnCriticalSection_leave(VnCriticalSection *cs)
+{
+	#if _WIN32
+
+	LeaveCriticalSection(&cs->handle);
+
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+
+	if (pthread_mutex_unlock(&cs->handle))
+		return E_UNKNOWN;
+
+	#else
+	#error "Unknown System"
+	#endif
+
+	return E_NONE;
+}

src/drivers/ins/vectornav/libvnc/src/vn/xplat/event.c

@@ -0,0 +1,239 @@
+/* Enable IEEE Std 1003.1b-1993 functionality required for clock_gettime. */
+#ifdef __linux__
+	/* Works for Ubuntu 15.10 */
+	#define _POSIX_C_SOURCE 199309L
+#elif defined __CYGWIN__
+	/* Works for Cygwin 2.4.0 64-bit */
+	#define _POSIX_TIMERS 	1
+#endif
+
+#include "vn/xplat/event.h"
+
+#if defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__
+	#include <time.h>
+	#include <errno.h>
+#endif
+
+#ifdef __APPLE__
+	#include <sys/time.h>
+	#include <pthread.h>
+	#include <mach/clock.h>
+	#include <mach/mach.h>
+#endif
+
+VnError VnEvent_initialize(VnEvent *e)
+{
+	#ifdef _WIN32
+
+	e->handle = CreateEvent(
+		NULL,
+		FALSE,
+		FALSE,
+		NULL);
+
+	if (e->handle == NULL)
+		return E_UNKNOWN;
+
+	#elif defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__
+
+	if (pthread_mutex_init(&e->mutex, NULL))
+		return E_UNKNOWN;
+
+	if (pthread_cond_init(&e->condition, NULL))
+		return E_UNKNOWN;
+
+	e->isTriggered = false;
+
+	#else
+		#error "Unknown System"
+	#endif
+
+	return E_NONE;
+}
+
+VnError VnEvent_wait(VnEvent *e)
+{
+	#ifdef _WIN32
+
+	DWORD result;
+
+	result = WaitForSingleObject(e->handle, 0xffffffff);
+
+	if (result == WAIT_OBJECT_0)
+		return E_NONE;
+
+	return E_UNKNOWN;
+
+	#elif defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__
+
+	if (pthread_mutex_lock(&e->mutex))
+		return E_UNKNOWN;
+
+	if (pthread_cond_wait(
+		&e->condition,
+		&e->mutex))
+		return E_UNKNOWN;
+
+	if (pthread_mutex_unlock(&e->mutex))
+		return E_UNKNOWN;
+
+	return E_NONE;
+
+	#else
+		#error "Unknown System"
+	#endif
+}
+
+VnError VnEvent_waitMs(VnEvent *e, uint32_t timeoutMs)
+{
+	#ifdef _WIN32
+
+	DWORD result;
+
+	result = WaitForSingleObject(e->handle, timeoutMs);
+
+	if (result == WAIT_OBJECT_0)
+		return E_SIGNALED;
+
+	if (result == WAIT_TIMEOUT)
+		return E_TIMEOUT;
+
+	return E_UNKNOWN;
+
+	#elif defined __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+
+	return VnEvent_waitUs(e, timeoutMs * 1000);
+
+	#else
+	#error "Unknown System"
+	#endif
+}
+
+VnError VnEvent_waitUs(VnEvent *e, uint32_t timeoutUs)
+{
+	#ifdef _WIN32
+
+	DWORD result;
+
+	result = WaitForSingleObject(
+		e->handle,
+		timeoutUs / 1000);
+
+	if (result == WAIT_OBJECT_0)
+		return E_SIGNALED;
+
+	if (result == WAIT_TIMEOUT)
+		return E_TIMEOUT;
+
+#elif (defined __linux__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__)
+
+	struct timespec now;
+	int errorCode;
+	uint32_t numOfSecs, numOfNanoseconds;
+
+	if (pthread_mutex_lock(&e->mutex))
+		return E_UNKNOWN;
+
+	if (clock_gettime(CLOCK_REALTIME, &now))
+		return E_UNKNOWN;
+
+	numOfSecs = timeoutUs / 1000000;
+	numOfNanoseconds = (timeoutUs % 1000000) * 1000;
+
+	now.tv_sec += numOfSecs;
+	now.tv_nsec += numOfNanoseconds;
+
+	if (now.tv_nsec > 1000000000)
+	{
+		now.tv_nsec %= 1000000000;
+		now.tv_sec++;
+	}
+
+	errorCode = pthread_cond_timedwait(
+		&e->condition,
+		&e->mutex,
+		&now);
+
+	if (pthread_mutex_unlock(&e->mutex))
+		return E_UNKNOWN;
+
+	if (!errorCode)
+		return E_SIGNALED;
+
+	if (errorCode == ETIMEDOUT)
+		return E_TIMEOUT;
+
+	#elif defined __APPLE__
+
+	pthread_mutex_lock(&e->mutex);
+
+	clock_serv_t cclock;
+	mach_struct timespec_t mts;
+	host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock);
+	clock_get_time(cclock, &mts);
+	mach_port_deallocate(mach_task_self(), cclock);
+
+	struct timespec now;
+	now.tv_sec = mts.tv_sec;
+	now.tv_nsec = mts.tv_nsec;
+
+	uint32_t numOfSecs = timeoutUs / 1000000;
+	uint32_t numOfNanoseconds = (timeoutUs % 1000000) * 1000;
+
+	now.tv_sec += numOfSecs;
+	now.tv_nsec += numOfNanoseconds;
+
+	if (now.tv_nsec > 1000000000)
+	{
+		now.tv_nsec %= 1000000000;
+		now.tv_sec++;
+	}
+
+	int errorCode = pthread_cond_timedwait(
+		&e->condition,
+		&e->mutex,
+		&now);
+
+	pthread_mutex_unlock(&e->mutex);
+
+	if (errorCode == 0)
+		return E_SIGNALED;
+
+	if (errorCode == ETIMEDOUT)
+		return E_TIMEOUT;
+
+	#else
+
+	#error "Unknown System"
+
+	#endif
+
+	return E_UNKNOWN;
+}
+
+VnError VnEvent_signal(VnEvent *e)
+{
+	#ifdef _WIN32
+
+	if (!SetEvent(e->handle))
+		return E_UNKNOWN;
+
+	#elif defined __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+
+	if (pthread_mutex_lock(&e->mutex))
+		return E_UNKNOWN;
+
+	e->isTriggered = true;
+
+	if (pthread_cond_signal(&e->condition))
+		return E_UNKNOWN;
+
+	if (pthread_mutex_unlock(&e->mutex))
+		return E_UNKNOWN;
+
+	#else
+	#error "Unknown System"
+	#endif
+
+	return E_NONE;
+}

src/drivers/ins/vectornav/libvnc/src/vn/xplat/serialport.c

@@ -0,0 +1,762 @@
+#include "vn/xplat/serialport.h"
+
+#if _WIN32
+	/* Nothing to do. */
+#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	#include <fcntl.h>
+	#include <errno.h>
+	#include <termios.h>
+	#include <string.h>
+	#include <unistd.h>
+	#include <sys/types.h>
+	#include <sys/select.h>
+#else
+	#error "Unknown System"
+#endif
+
+#define NUMBER_OF_BYTES_TO_PURGE_ON_OPENING_SERIAL_PORT 100
+#define WAIT_TIME_FOR_SERIAL_PORT_READS_MS 100
+
+/* Private declarations. */
+void VnSerialPort_purgeFirstDataBytesFromSerialPort(VnSerialPort *serialport);
+void VnSerialPort_startSerialPortNotificationsThread(VnSerialPort *serialport);
+void VnSerialPort_stopSerialPortNotificationsThread(VnSerialPort *serialport);
+void VnSerialPort_handleSerialPortNotifications(void* data);
+void VnSerialPort_onDataReceived(VnSerialPort *serialport);
+VnError VnSerialPort_open_internal(VnSerialPort *serialport, char const *portName, uint32_t baudrate, bool checkAndToggleIsOpenFlag);
+VnError VnSerialPort_close_internal(VnSerialPort *serialport, bool checkAndToggleIsOpenFlag);
+
+VnError VnSerialPort_initialize(VnSerialPort *sp)
+{
+	#if _WIN32
+	sp->handle = NULL;
+	VnCriticalSection_initialize(&sp->readWriteCS);
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	sp->handle = -1;
+	#else
+	#error "Unknown System"
+	#endif
+
+	sp->purgeFirstDataBytesWhenSerialPortIsFirstOpened = true;
+	sp->dataReceivedHandler = NULL;
+	sp->dataReceivedHandlerUserData = NULL;
+	sp->continueHandlingSerialPortEvents = false;
+	sp->numberOfReceiveDataDroppedSections = 0;
+
+	VnCriticalSection_initialize(&sp->dataReceivedHandlerCriticalSection);
+
+	return E_NONE;
+}
+
+VnError VnSerialPort_open_internal(VnSerialPort *serialport, char const *portName, uint32_t baudrate, bool checkAndToggleIsOpenFlag)
+{
+	#if _WIN32
+	DCB config;
+	const char *preName = "\\\\.\\";
+	char *fullName;
+	size_t fullNameLen;
+	COMMTIMEOUTS comTimeOut;
+	#ifdef UNICODE
+	WCHAR wFullName[0x100];
+	#endif
+
+	#elif (defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__)
+
+	struct termios portSettings;
+	int portFd = -1;
+	tcflag_t baudrateFlag;
+
+	#endif
+
+	#if VN_HAVE_SECURE_CRT
+	strcpy_s(serialport->portName, sizeof(serialport->portName), portName);
+	#else
+	strcpy(serialport->portName, portName);
+	#endif
+
+	if (checkAndToggleIsOpenFlag && VnSerialPort_isOpen(serialport))
+		return E_INVALID_OPERATION;
+
+	#if _WIN32
+
+	fullNameLen = strlen(preName) + strlen(portName) + 1;
+	fullName = (char*) malloc(fullNameLen);
+	#if VN_HAVE_SECURE_CRT
+	strcpy_s(fullName, fullNameLen, preName);
+	strcat_s(fullName, fullNameLen, portName);
+	#else
+	strcpy(fullName, preName);
+	strcat(fullName, portName);
+	#endif
+
+	#ifdef UNICODE
+	mbstowcs(wFullName, fullName, strlen(fullName) + 1);
+	#endif
+
+	serialport->handle = CreateFile(
+		#ifdef UNICODE
+		wFullName,
+		#else
+		fullName,
+		#endif
+		GENERIC_READ | GENERIC_WRITE,
+		0,
+		NULL,
+		OPEN_EXISTING,
+		FILE_FLAG_OVERLAPPED,
+		NULL);
+
+	free(fullName);
+
+	if (serialport->handle == INVALID_HANDLE_VALUE)
+	{
+		DWORD error = GetLastError();
+
+		if (error == ERROR_ACCESS_DENIED)
+			/* Port is probably already open. */
+			return E_INVALID_OPERATION;
+
+		if (error == ERROR_FILE_NOT_FOUND)
+			/* Port probably does not exist. */
+			return E_NOT_FOUND;
+
+		return E_UNKNOWN;
+	}
+
+	/* Set the state of the COM port. */
+	if (!GetCommState(serialport->handle, &config))
+	{
+		DWORD error = GetLastError();
+
+		if (error != ERROR_OPERATION_ABORTED)
+			return E_UNKNOWN;
+
+		/* Try clearing this error. */
+		if (!ClearCommError(serialport->handle, &error, NULL))
+			return E_UNKNOWN;
+
+		/* Retry the operation. */
+		if (!GetCommState(serialport->handle, &config))
+			return E_UNKNOWN;
+	}
+
+	config.BaudRate = baudrate;
+	config.StopBits = ONESTOPBIT;
+	config.Parity = NOPARITY;
+	config.ByteSize = 8;
+	config.fAbortOnError = 0;
+
+	if (!SetCommState(serialport->handle, &config))
+	{
+		DWORD error = GetLastError();
+
+		if (error == ERROR_INVALID_PARAMETER)
+		{
+			if (!CloseHandle(serialport->handle))
+				return E_UNKNOWN;
+
+			return E_INVALID_VALUE;
+		}
+
+		if (error != ERROR_OPERATION_ABORTED)
+			return E_UNKNOWN;
+
+		/* Try clearing this error. */
+		if (!ClearCommError(serialport->handle, &error, NULL))
+			return E_UNKNOWN;
+
+		/* Retry this operation. */
+		if (!SetCommState(serialport->handle, &config))
+			return E_UNKNOWN;
+	}
+
+	comTimeOut.ReadIntervalTimeout = 0;
+	comTimeOut.ReadTotalTimeoutMultiplier = 0;
+	comTimeOut.ReadTotalTimeoutConstant = 1;
+	comTimeOut.WriteTotalTimeoutMultiplier = 3;
+	comTimeOut.WriteTotalTimeoutConstant = 2;
+
+	if (!SetCommTimeouts(serialport->handle, &comTimeOut))
+	{
+		DWORD error = GetLastError();
+
+		if (error != ERROR_OPERATION_ABORTED)
+			return E_UNKNOWN;
+
+		/* Try clearing this error. */
+		if (!ClearCommError(serialport->handle, &error, NULL))
+			return E_UNKNOWN;
+
+		/* Retry the operation. */
+		if (!SetCommTimeouts(serialport->handle, &comTimeOut))
+			return E_UNKNOWN;
+	}
+
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
+
+	portFd = open(
+		portName,
+		#if __linux__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+		O_RDWR | O_NOCTTY);
+		#elif __APPLE__
+		O_RDWR | O_NOCTTY | O_NONBLOCK);
+		#else
+		#error "Unknown System"
+		#endif
+
+	if (portFd == -1)
+	{
+		switch (errno)
+		{
+		case EACCES:
+			return E_PERMISSION_DENIED;
+		case ENXIO:
+		case ENOTDIR:
+		case ENOENT:
+			return E_NOT_FOUND;
+		default:
+			return E_UNKNOWN;
+		}
+	}
+
+	memset(
+		&portSettings,
+		0,
+		sizeof(portSettings));
+
+	switch (baudrate)
+	{
+		case 9600:
+			baudrateFlag = B9600;
+			break;
+		case 19200:
+			baudrateFlag = B19200;
+			break;
+		case 38400:
+			baudrateFlag = B38400;
+			break;
+		case 57600:
+			baudrateFlag = B57600;
+			break;
+		case 115200:
+			baudrateFlag = B115200;
+			break;
+
+		/* QNX does not have higher baudrates defined. */
+		#if !defined(__QNXNTO__)
+
+		case 230400:
+			baudrateFlag = B230400;
+			break;
+
+		/* Not available on Mac OS X??? */
+		#if !defined(__APPLE__)
+
+		case 460800:
+			baudrateFlag = B460800;
+			break;
+		case 921600:
+			baudrateFlag = B921600;
+			break;
+
+		#endif
+
+		#endif
+
+		default:
+			return E_INVALID_VALUE;
+	}
+
+	/* Set baudrate, 8n1, no modem control, enable receiving characters. */
+	#if __linux__ || __QNXNTO__ || __CYGWIN__ || defined __NUTTX__
+	portSettings.c_cflag = baudrateFlag;
+	#elif defined(__APPLE__)
+	cfsetspeed(&portSettings, baudrateFlag);
+	#endif
+	portSettings.c_cflag |= CS8 | CLOCAL | CREAD;
+
+	portSettings.c_iflag = IGNPAR;		/* Ignore bytes with parity errors. */
+	portSettings.c_oflag = 0;			/* Enable raw data output. */
+	portSettings.c_cc[VTIME] = 0;		/* Do not use inter-character timer. */
+	portSettings.c_cc[VMIN] = 0;		/* Block on reads until 0 character is received. */
+
+	/* Clear the COM port buffers. */
+	if (tcflush(portFd, TCIFLUSH) != 0)
+		return E_UNKNOWN;
+
+	if (tcsetattr(portFd, TCSANOW, &portSettings) != 0)
+		return E_UNKNOWN;
+
+	serialport->handle = portFd;
+
+	#else
+	#error "Unknown System"
+	#endif
+
+	if (serialport->purgeFirstDataBytesWhenSerialPortIsFirstOpened)
+		VnSerialPort_purgeFirstDataBytesFromSerialPort(serialport);
+
+	VnSerialPort_startSerialPortNotificationsThread(serialport);
+
+	return E_NONE;
+}
+
+VnError VnSerialPort_close_internal(VnSerialPort *serialport, bool checkAndToggleIsOpenFlag)
+{
+	if (checkAndToggleIsOpenFlag && !VnSerialPort_isOpen(serialport))
+		return E_INVALID_OPERATION;
+
+	VnSerialPort_stopSerialPortNotificationsThread(serialport);
+
+	#if _WIN32
+
+	if (!CloseHandle(serialport->handle))
+		return E_UNKNOWN;
+
+	serialport->handle = NULL;
+
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+
+	if (close(serialport->handle) == -1)
+		return E_UNKNOWN;
+
+	serialport->handle = -1;
+
+	#else
+	#error "Unknown System"
+	#endif
+
+	return E_NONE;
+}
+
+VnError VnSerialPort_closeAfterUserUnpluggedSerialPort(VnSerialPort *serialport)
+{
+	#if _WIN32
+
+	if (!CloseHandle(serialport->handle))
+		return E_UNKNOWN;
+
+	serialport->handle = NULL;
+
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+
+	if (close(serialport->handle) == -1)
+		return E_UNKNOWN;
+
+	serialport->handle = -1;
+
+	#else
+	#error "Unknown System"
+	#endif
+
+	return E_NONE;
+}
+
+VnError VnSerialPort_open(VnSerialPort *serialport, char const *portName, uint32_t baudrate)
+{
+	return VnSerialPort_open_internal(serialport, portName, baudrate, true);
+}
+
+bool VnSerialPort_isOpen(VnSerialPort *serialport)
+{
+	#if defined(_WIN32)
+	return serialport->handle != NULL;
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	return serialport->handle != -1;
+	#else
+	#error "Unknown System"
+	#endif
+}
+
+VnError VnSerialPort_changeBaudrate(VnSerialPort *serialport, uint32_t baudrate)
+{
+	VnError error;
+
+	if (!VnSerialPort_isOpen(serialport))
+		return E_INVALID_OPERATION;
+
+	if ((error = VnSerialPort_close_internal(serialport, false)) != E_NONE)
+		return error;
+
+	return VnSerialPort_open_internal(serialport, serialport->portName, baudrate, false);
+}
+
+void VnSerialPort_purgeFirstDataBytesFromSerialPort(VnSerialPort *serialport)
+{
+	char buffer[NUMBER_OF_BYTES_TO_PURGE_ON_OPENING_SERIAL_PORT];
+	size_t numOfBytesRead;
+
+	VnSerialPort_read(serialport, buffer, NUMBER_OF_BYTES_TO_PURGE_ON_OPENING_SERIAL_PORT, &numOfBytesRead);
+}
+
+VnError VnSerialPort_read(VnSerialPort *serialport, char *buffer, size_t numOfBytesToRead, size_t *numOfBytesActuallyRead)
+{
+	#if defined(_WIN32)
+	OVERLAPPED overlapped;
+	BOOL result;
+	DWORD numOfBytesTransferred;
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	int result;
+	#else
+	#error "Unknown System"
+	#endif
+
+	if (!VnSerialPort_isOpen(serialport))
+		return E_INVALID_OPERATION;
+
+	#if _WIN32
+
+	memset(&overlapped, 0, sizeof(OVERLAPPED));
+
+	VnCriticalSection_enter(&serialport->readWriteCS);
+
+	result = ReadFile(
+		serialport->handle,
+		buffer,
+		numOfBytesToRead,
+		NULL,
+		&overlapped);
+
+	if (!result && GetLastError() != ERROR_IO_PENDING)
+	{
+		VnCriticalSection_leave(&serialport->readWriteCS);
+		return E_UNKNOWN;
+	}
+
+	result = GetOverlappedResult(
+		serialport->handle,
+		&overlapped,
+		&numOfBytesTransferred,
+		TRUE);
+
+	VnCriticalSection_leave(&serialport->readWriteCS);
+
+	*numOfBytesActuallyRead = numOfBytesTransferred;
+
+	if (!result)
+		return E_UNKNOWN;
+
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+
+	result = read(
+		serialport->handle,
+		buffer,
+		numOfBytesToRead);
+
+	if (result == -1)
+		return E_UNKNOWN;
+
+	*numOfBytesActuallyRead = result;
+
+	#else
+	#error "Unknown System"
+	#endif
+
+	return E_NONE;
+}
+
+VnError VnSerialPort_write(VnSerialPort *sp, char const *data, size_t length)
+{
+	#if defined(_WIN32)
+	DWORD numOfBytesWritten;
+	BOOL result;
+	OVERLAPPED overlapped;
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	size_t numOfBytesWritten;
+	#else
+	#error "Unknown System"
+	#endif
+
+	if (!VnSerialPort_isOpen(sp))
+		return E_INVALID_OPERATION;
+
+	#if defined(_WIN32)
+
+	memset(&overlapped, 0, sizeof(OVERLAPPED));
+
+	VnCriticalSection_enter(&sp->readWriteCS);
+
+	result = WriteFile(
+		sp->handle,
+		data,
+		length,
+		NULL,
+		&overlapped);
+
+	if (!result && GetLastError() != ERROR_IO_PENDING)
+	{
+		VnCriticalSection_leave(&sp->readWriteCS);
+		return E_UNKNOWN;
+	}
+
+	result = GetOverlappedResult(
+		sp->handle,
+		&overlapped,
+		&numOfBytesWritten,
+		TRUE);
+
+	if (!result)
+	{
+		VnCriticalSection_leave(&sp->readWriteCS);
+		return E_UNKNOWN;
+	}
+
+	result = FlushFileBuffers(sp->handle);
+
+	VnCriticalSection_leave(&sp->readWriteCS);
+
+	if (!result)
+		return E_UNKNOWN;
+
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+
+	numOfBytesWritten = write(
+		sp->handle,
+		data,
+		length);
+
+	if (numOfBytesWritten == -1)
+		return E_UNKNOWN;
+
+	#else
+	#error "Unknown System"
+	#endif
+
+	return E_NONE;
+}
+
+VnError VnSerialPort_close(VnSerialPort *serialport)
+{
+	return VnSerialPort_close_internal(serialport, true);
+}
+
+void VnSerialPort_startSerialPortNotificationsThread(VnSerialPort *serialport)
+{
+	serialport->continueHandlingSerialPortEvents = true;
+
+	VnThread_startNew(&serialport->serialPortNotificationsThread, VnSerialPort_handleSerialPortNotifications, serialport);
+}
+
+void VnSerialPort_stopSerialPortNotificationsThread(VnSerialPort *serialport)
+{
+	serialport->continueHandlingSerialPortEvents = false;
+
+	VnThread_join(&serialport->serialPortNotificationsThread);
+}
+
+void VnSerialPort_handleSerialPortNotifications(void* routineData)
+{
+	bool userUnpluggedUsbCable = false;
+	VnSerialPort *sp = (VnSerialPort*) routineData;
+
+	#if _WIN32
+
+	OVERLAPPED overlapped;
+
+	memset(&overlapped, 0, sizeof(OVERLAPPED));
+
+	overlapped.hEvent = CreateEvent(
+		NULL,
+		false,
+		false,
+		NULL);
+
+	SetCommMask(
+		sp->handle,
+		EV_RXCHAR | EV_ERR | EV_RX80FULL);
+
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+
+	fd_set readfs;
+	int error;
+	struct timeval readWaitTime;
+
+	#else
+	#error "Unknown System"
+	#endif
+
+	while (sp->continueHandlingSerialPortEvents)
+	{
+		#if _WIN32
+
+		DWORD mask = 0;
+		DWORD temp = 0;
+		DWORD waitResult;
+		BOOL result;
+
+		result = WaitCommEvent(
+			sp->handle,
+			&mask,
+			&overlapped);
+
+		if (result)
+		{
+			VnSerialPort_onDataReceived(sp);
+
+			continue;
+		}
+
+		if (GetLastError() != ERROR_IO_PENDING)
+			/* Something unexpected happened. */
+			break;
+
+		KeepWaiting:
+
+		/* We need to wait for the event to occur. */
+		waitResult = WaitForSingleObject(
+			overlapped.hEvent,
+			WAIT_TIME_FOR_SERIAL_PORT_READS_MS);
+
+		if (!sp->continueHandlingSerialPortEvents)
+			break;
+
+		if (waitResult == WAIT_TIMEOUT)
+			goto KeepWaiting;
+
+		if (waitResult != WAIT_OBJECT_0)
+			/* Something unexpected happened. */
+			break;
+
+		if (!GetOverlappedResult(
+			sp->handle,
+			&overlapped,
+			&temp,
+			TRUE))
+		{
+			if (GetLastError() == ERROR_OPERATION_ABORTED)
+			{
+				/* User probably uplugged UART-to-USB cable. */
+				sp->continueHandlingSerialPortEvents = false;
+				userUnpluggedUsbCable = true;
+			}
+
+			/* Something unexpected happened. */
+			break;
+		}
+
+		if (mask & EV_RXCHAR)
+		{
+			VnSerialPort_onDataReceived(sp);
+
+			continue;
+		}
+
+		if (mask & EV_RX80FULL)
+		{
+			/* We assume the RX buffer was overrun. */
+			sp->numberOfReceiveDataDroppedSections++;
+
+			continue;
+		}
+
+		if (mask & EV_ERR)
+		{
+			DWORD spErrors;
+			COMSTAT comStat;
+
+			if (!ClearCommError(
+				sp->handle,
+				&spErrors,
+				&comStat))
+			{
+				/* Something unexpected happened. */
+				break;
+			}
+
+			if ((spErrors & CE_OVERRUN) || (spErrors & CE_RXOVER))
+			{
+				/* The serial buffer RX buffer was overrun. */
+				sp->numberOfReceiveDataDroppedSections++;
+			}
+
+			continue;
+		}
+
+		#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+
+		FD_ZERO(&readfs);
+		FD_SET(sp->handle, &readfs);
+
+		/* Select sets the values in readWaitTime. */
+		readWaitTime.tv_sec = 0;
+		readWaitTime.tv_usec = WAIT_TIME_FOR_SERIAL_PORT_READS_MS * 1000;
+
+		error = select(
+			sp->handle + 1,
+			&readfs,
+			NULL,
+			NULL,
+			&readWaitTime);
+
+		if (error == -1)
+			/* Something unexpected happened. */
+			break;
+
+		if (!FD_ISSET(sp->handle, &readfs))
+			continue;
+
+		VnSerialPort_onDataReceived(sp);
+
+		#else
+		#error "Unknown System"
+		#endif
+
+	}
+
+	if (sp->continueHandlingSerialPortEvents)
+		; /* An error must have occurred. Do nothing for now. */
+
+	#if _WIN32
+
+	if (!userUnpluggedUsbCable)
+	{
+		SetCommMask(
+			sp->handle,
+			0);
+	}
+
+	#endif
+
+	if (userUnpluggedUsbCable)
+		VnSerialPort_closeAfterUserUnpluggedSerialPort(sp);
+}
+
+void VnSerialPort_onDataReceived(VnSerialPort *sp)
+{
+	VnCriticalSection_enter(&sp->dataReceivedHandlerCriticalSection);
+
+	if (sp->dataReceivedHandler != NULL)
+		sp->dataReceivedHandler(sp->dataReceivedHandlerUserData);
+
+	VnCriticalSection_leave(&sp->dataReceivedHandlerCriticalSection);
+}
+
+VnError VnSerialPort_registerDataReceivedHandler(VnSerialPort *sp, VnSerialPort_DataReceivedHandler handler, void *userData)
+{
+	if (sp->dataReceivedHandler != NULL)
+		return E_INVALID_OPERATION;
+
+	VnCriticalSection_enter(&sp->dataReceivedHandlerCriticalSection);
+
+	sp->dataReceivedHandler = handler;
+	sp->dataReceivedHandlerUserData = userData;
+
+	VnCriticalSection_leave(&sp->dataReceivedHandlerCriticalSection);
+
+	return E_NONE;
+}
+
+VnError VnSerialPort_unregisterDataReceivedHandler(VnSerialPort *sp)
+{
+	if (sp->dataReceivedHandler == NULL)
+		return E_INVALID_OPERATION;
+
+	VnCriticalSection_enter(&sp->dataReceivedHandlerCriticalSection);
+
+	sp->dataReceivedHandler = NULL;
+	sp->dataReceivedHandlerUserData = NULL;
+
+	VnCriticalSection_leave(&sp->dataReceivedHandlerCriticalSection);
+
+	return E_NONE;
+}

src/drivers/ins/vectornav/libvnc/src/vn/xplat/thread.c

@@ -0,0 +1,157 @@
+#define _BSD_SOURCE
+
+#include "vn/xplat/thread.h"
+
+#if (defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__)
+	#include <stdlib.h>
+	#include <unistd.h>
+	#include <stddef.h>
+	#include <pthread.h>
+#endif
+
+#undef __cplusplus
+
+typedef struct
+{
+	VnThread_StartRoutine startRoutine;
+	void *routineData;
+} VnThreadStarter;
+
+#if _WIN32
+
+DWORD WINAPI VnThread_intermediateStartRoutine(LPVOID userData)
+{
+	VnThreadStarter *starter = (VnThreadStarter*) userData;
+
+	VnThread_StartRoutine routine = starter->startRoutine;
+	void *routineData = starter->routineData;
+
+	free(starter);
+
+	routine(routineData);
+
+	return 0;
+}
+
+#elif (defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__)
+
+void* VnThread_intermediateStartRoutine(void* data)
+{
+	VnThreadStarter *starter = (VnThreadStarter*) data;
+
+	VnThread_StartRoutine routine = starter->startRoutine;
+	void *routineData = starter->routineData;
+
+	free(starter);
+
+	routine(routineData);
+
+	return NULL;
+}
+
+#endif
+
+VnError VnThread_startNew(VnThread *thread, VnThread_StartRoutine startRoutine, void* routineData)
+{
+	#if (defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__)
+	int errorCode;
+	#endif
+
+	VnThreadStarter *starter = (VnThreadStarter*) malloc(sizeof(VnThreadStarter));
+
+	starter->startRoutine = startRoutine;
+	starter->routineData = routineData;
+
+	#if _WIN32
+
+	thread->handle = CreateThread(
+		NULL,
+		0,
+		VnThread_intermediateStartRoutine,
+		starter,
+		0,
+		NULL);
+
+	if (thread->handle == NULL)
+		return E_UNKNOWN;
+
+	#elif (defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__)
+
+
+	errorCode = pthread_create(
+		&(thread->handle),
+		NULL,
+		VnThread_intermediateStartRoutine,
+		starter);
+
+	if (errorCode != 0)
+		return E_UNKNOWN;
+
+	#else
+	#error "Unknown System"
+	#endif
+
+	return E_NONE;
+}
+
+VnError VnThread_join(VnThread *thread)
+{
+	#if _WIN32
+
+	WaitForSingleObject(
+		thread->handle,
+		INFINITE);
+
+	#elif (defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__)
+
+	int error = pthread_join(
+		thread->handle,
+		NULL);
+
+	if (error != 0)
+		return E_UNKNOWN;
+
+	#else
+	#error "Unknown System"
+	#endif
+
+	return E_NONE;
+}
+
+void VnThread_sleepSec(uint32_t numOfSecsToSleep)
+{
+	#if _WIN32
+	Sleep(numOfSecsToSleep * 1000);
+	#elif (defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__)
+	sleep(numOfSecsToSleep);
+	#else
+	#error "Unknown System"
+	#endif
+}
+
+void VnThread_sleepMs(uint32_t numOfMsToSleep)
+{
+	#if _WIN32
+	Sleep(numOfMsToSleep);
+	#elif (defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__)
+	VnThread_sleepUs(numOfMsToSleep * 1000);
+	#else
+	#error "Unknown System"
+	#endif
+}
+
+void VnThread_sleepUs(uint32_t numOfUsToSleep)
+{
+	#if _WIN32
+	/* Not implemented. */
+	int exitValue = numOfUsToSleep;
+	exitValue = -1;
+	exit(exitValue);
+
+	#elif (defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__)
+	usleep(numOfUsToSleep);
+
+	#else
+	#error "Unknown System"
+	#endif
+}

src/drivers/ins/vectornav/libvnc/src/vn/xplat/time.c

@@ -0,0 +1,129 @@
+/* Enable IEEE Std 1003.1b-1993 functionality required for clock_gettime. */
+#ifdef __linux__
+	/* Works for Ubuntu 15.10 */
+	#define _POSIX_C_SOURCE 199309L
+#elif defined __CYGWIN__
+	/* Works for Cygwin 2.4.0 64-bit */
+	#define _POSIX_TIMERS 				1
+	#define _POSIX_MONOTONIC_CLOCK		200112L
+#endif
+
+#include "vn/xplat/time.h"
+
+#if (defined __linux__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__)
+	#include <time.h>
+#elif defined __APPLE__
+	#include <mach/clock.h>
+	#include <mach/mach.h>
+#endif
+
+VnError VnStopwatch_initializeAndStart(VnStopwatch *sw)
+{
+	#if _WIN32
+
+	sw->pcFrequency = 0;
+	sw->counterStart = -1;
+
+	#elif (defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__)
+
+	sw->clockStart = -1;
+
+	#else
+	#error "Unknown System"
+	#endif
+
+	/* Start the stopwatch. */
+	return VnStopwatch_reset(sw);
+}
+
+VnError VnStopwatch_reset(VnStopwatch *sw)
+{
+	#if _WIN32
+
+	LARGE_INTEGER li;
+	if(!QueryPerformanceFrequency(&li))
+		/* The hardware must not support a high-resolution performance counter. */
+		return E_NOT_SUPPORTED;
+
+	sw->pcFrequency = li.QuadPart / 1000.0;
+
+	QueryPerformanceCounter(&li);
+
+	sw->counterStart = li.QuadPart;
+
+	#elif (defined __linux__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__)
+
+	struct timespec time;
+	int error;
+
+	error = clock_gettime(CLOCK_MONOTONIC, &time);
+
+	if (error)
+		return E_UNKNOWN;
+
+	sw->clockStart = (time.tv_sec * 1000.0) + (time.tv_nsec / 1000000.0);
+
+	#elif defined __APPLE__
+
+	clock_serv_t cclock;
+	mach_struct timespec_t mts;
+
+	host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock);
+	clock_get_time(cclock, &mts);
+	mach_port_deallocate(mach_task_self(), cclock);
+
+	sw->clockStart = (mts.tv_sec * 1000.0) + (mts.tv_nsec / 1000000.0);
+
+	#else
+	#error "Unknown System"
+	#endif
+
+	return E_NONE;
+}
+
+VnError VnStopwatch_elapsedMs(VnStopwatch *sw, float *elapsedMs)
+{
+	#if _WIN32
+
+	LARGE_INTEGER li;
+
+	if (sw->counterStart == -1)
+		return E_UNKNOWN;
+
+	QueryPerformanceCounter(&li);
+
+	*elapsedMs = (float) ((li.QuadPart - sw->counterStart) / sw->pcFrequency);
+
+	#elif (defined __linux__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__)
+
+	struct timespec time;
+	int error;
+
+	if (sw->clockStart < 0)
+		/* Clock not started. */
+		return E_INVALID_OPERATION;
+
+	error = clock_gettime(CLOCK_MONOTONIC, &time);
+
+	if (error)
+		return E_UNKNOWN;
+
+	*elapsedMs = (time.tv_sec * 1000.0) + (time.tv_nsec / 1000000.0) - sw->clockStart;
+
+	#elif defined __APPLE__
+
+	clock_serv_t cclock;
+	mach_struct timespec_t mts;
+
+	host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock);
+	clock_get_time(cclock, &mts);
+	mach_port_deallocate(mach_task_self(), cclock);
+
+	return (mts.tv_sec * 1000.0) + (mts.tv_nsec / 1000000.0) - sw->clockStart;
+
+	#else
+	#error "Unknown System"
+	#endif
+
+	return E_NONE;
+}

src/drivers/ins/vectornav/module.yaml

@@ -0,0 +1,7 @@
+module_name: VectorNav (VN-100, VN-200, VN-300)
+serial_config:
+    - command: vectornav start -d ${SERIAL_DEV}
+      port_config_param:
+        name: SENS_VN_CFG
+        group: Sensors
+

src/modules/logger/logged_topics.cpp

@@ -59,6 +59,9 @@ void LoggedTopics::add_default_topics()
 	add_topic("cellular_status", 200);
 	add_topic("commander_state");
 	add_topic("cpuload");
+	add_optional_topic("external_ins_attitude");
+	add_optional_topic("external_ins_global_position");
+	add_optional_topic("external_ins_local_position");
 	add_optional_topic("esc_status", 250);
 	add_topic("failure_detector_status", 100);
 	add_topic("failsafe_flags");