diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index 6681bf84ac..66ad713801 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/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 \ diff --git a/Tools/px4moduledoc/srcparser.py b/Tools/px4moduledoc/srcparser.py index c936ad7a81..59184b2b95 100644 --- a/Tools/px4moduledoc/srcparser.py +++ b/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 diff --git a/boards/px4/fmu-v6c/default.px4board b/boards/px4/fmu-v6c/default.px4board index 0e07d9dd60..dda6753259 100644 --- a/boards/px4/fmu-v6c/default.px4board +++ b/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 diff --git a/msg/VehicleAttitude.msg b/msg/VehicleAttitude.msg index 6eeb20a21e..46e1fc0bcb 100644 --- a/msg/VehicleAttitude.msg +++ b/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 diff --git a/msg/VehicleGlobalPosition.msg b/msg/VehicleGlobalPosition.msg index ce16daecc8..77244473e2 100644 --- a/msg/VehicleGlobalPosition.msg +++ b/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 diff --git a/msg/VehicleLocalPosition.msg b/msg/VehicleLocalPosition.msg index a638bbb61c..4f9238123c 100644 --- a/msg/VehicleLocalPosition.msg +++ b/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 diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index a0d109229d..b264086bd6 100644 --- a/src/drivers/drv_sensor.h +++ b/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 diff --git a/src/drivers/ins/Kconfig b/src/drivers/ins/Kconfig new file mode 100644 index 0000000000..5c215e3cca --- /dev/null +++ b/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 diff --git a/src/drivers/ins/vectornav/CMakeLists.txt b/src/drivers/ins/vectornav/CMakeLists.txt new file mode 100644 index 0000000000..7e77611333 --- /dev/null +++ b/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 + ) diff --git a/src/drivers/ins/vectornav/Kconfig b/src/drivers/ins/vectornav/Kconfig new file mode 100644 index 0000000000..8b1826f9e0 --- /dev/null +++ b/src/drivers/ins/vectornav/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_INS_VECTORNAV + bool "vectornav" + default n + ---help--- + Enable support for vectornav diff --git a/src/drivers/ins/vectornav/VectorNav.cpp b/src/drivers/ins/vectornav/VectorNav.cpp new file mode 100644 index 0000000000..2aa61a7f0d --- /dev/null +++ b/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 +#include + +#include + +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(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); +} diff --git a/src/drivers/ins/vectornav/VectorNav.hpp b/src/drivers/ins/vectornav/VectorNav.hpp new file mode 100644 index 0000000000..bc1244a009 --- /dev/null +++ b/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 +#include +#include + +extern "C" { +#include "vn/sensors/searcher.h" + +#include "vn/sensors/compositedata.h" +} + +#include "vn/sensors.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class VectorNav : public ModuleBase, 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_pub{ORB_ID(sensor_baro)}; + uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; + + uORB::PublicationMulti _attitude_pub{ORB_ID(external_ins_attitude)}; + uORB::PublicationMulti _local_position_pub{ORB_ID(external_ins_local_position)}; + uORB::PublicationMulti _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. +}; diff --git a/src/drivers/ins/vectornav/libvnc/CMakeLists.txt b/src/drivers/ins/vectornav/libvnc/CMakeLists.txt new file mode 100644 index 0000000000..7e9bc82ac0 --- /dev/null +++ b/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() diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/bool.h b/src/drivers/ins/vectornav/libvnc/include/vn/bool.h new file mode 100644 index 0000000000..eee847fe3c --- /dev/null +++ b/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 + #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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/const.h b/src/drivers/ins/vectornav/libvnc/include/vn/const.h new file mode 100644 index 0000000000..96acaf2c21 --- /dev/null +++ b/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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/conv.h b/src/drivers/ins/vectornav/libvnc/include/vn/conv.h new file mode 100644 index 0000000000..58d1b48af6 --- /dev/null +++ b/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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/enum.h b/src/drivers/ins/vectornav/libvnc/include/vn/enum.h new file mode 100644 index 0000000000..ca59b176c4 --- /dev/null +++ b/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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/error.h b/src/drivers/ins/vectornav/libvnc/include/vn/error.h new file mode 100644 index 0000000000..cdbdbe7abb --- /dev/null +++ b/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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/error_detection.h b/src/drivers/ins/vectornav/libvnc/include/vn/error_detection.h new file mode 100644 index 0000000000..411f54d51c --- /dev/null +++ b/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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/int.h b/src/drivers/ins/vectornav/libvnc/include/vn/int.h new file mode 100644 index 0000000000..e947440236 --- /dev/null +++ b/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 +#endif + +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#endif diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/math/matrix.h b/src/drivers/ins/vectornav/libvnc/include/vn/math/matrix.h new file mode 100644 index 0000000000..6bbf62b97c --- /dev/null +++ b/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 float. */ +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 float. */ +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 + diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/math/vector.h b/src/drivers/ins/vectornav/libvnc/include/vn/math/vector.h new file mode 100644 index 0000000000..a363450c2d --- /dev/null +++ b/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 +* float. */ +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 +* double. */ +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 +* float. */ +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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/protocol/common.h b/src/drivers/ins/vectornav/libvnc/include/vn/protocol/common.h new file mode 100644 index 0000000000..dc0fa2b728 --- /dev/null +++ b/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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/protocol/spi.h b/src/drivers/ins/vectornav/libvnc/include/vn/protocol/spi.h new file mode 100644 index 0000000000..18fcfe7669 --- /dev/null +++ b/src/drivers/ins/vectornav/libvnc/include/vn/protocol/spi.h @@ -0,0 +1,2481 @@ +#ifndef _VNSPI_H_ +#define _VNSPI_H_ + +#include + +#include "vn/int.h" +#include "vn/error.h" +#include "vn/math/vector.h" +#include "vn/math/matrix.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** \brief Generates a command to write settings. +* +* \param[in] buffer Caller provided buffer to place the generated command. +* \param[in/out] size Number of bytes available in the buffer.Will contain on output the number of bytes in the command to send. +* \param[in] desiredLength The total number of bytes to pad with 0x00 should +* the total constructed length of the command be less than the +* desiredLength.This is useful back - to - back command where the +* desiredLength will be the responseSize of the previous command sent. +* \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. +* \return Indicates any errors encountered. */ +VnError VnSpi_genWriteSettings( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to restore factory settings. +* +* \param[in] buffer Caller provided buffer to place the generated command. +* \param[in/out] size Number of bytes available in the buffer.Will contain on output the number of bytes in the command to send. +* \param[in] desiredLength The total number of bytes to pad with 0x00 should +* the total constructed length of the command be less than the +* desiredLength.This is useful back - to - back command where the +* desiredLength will be the responseSize of the previous command sent. +* \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. +* \return Indicates any errors encountered. */ +VnError VnSpi_genRestorFactorySettings( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to tare the sensor. +* +* \param[in] buffer Caller provided buffer to place the generated command. +* \param[in/out] size Number of bytes available in the buffer.Will contain on output the number of bytes in the command to send. +* \param[in] desiredLength The total number of bytes to pad with 0x00 should +* the total constructed length of the command be less than the +* desiredLength.This is useful back - to - back command where the +* desiredLength will be the responseSize of the previous command sent. +* \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. +* \return Indicates any errors encountered. */ +VnError VnSpi_genTare( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to reset the sensor. +* +* \param[in] buffer Caller provided buffer to place the generated command. +* \param[in/out] size Number of bytes available in the buffer.Will contain on output the number of bytes in the command to send. +* \param[in] desiredLength The total number of bytes to pad with 0x00 should +* the total constructed length of the command be less than the +* desiredLength.This is useful back - to - back command where the +* desiredLength will be the responseSize of the previous command sent. +* \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. +* \return Indicates any errors encountered. */ +VnError VnSpi_genReset( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generic function for making register read commands. +* +* \param[out] buffer Caller provided buffer to place the generated command. +* \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. +* \param[in] regId The register ID to generate the read command for. +* \param[in] desiredLength The total number of bytes to pad with 0x00 should +* the total constructed length of the command be less than the +* desiredLength. This is useful back-to-back command where the +* desiredLength will be the responseSize of the previous command sent. +* \return Indicates any error encountered. */ +VnError VnSpi_genRead( + char* buffer, + size_t* size, + uint8_t regId, + size_t desiredLength); + +/** \brief Parses a response from reading the User Tag register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] tag The register's Tag field. +* \param[in] tagLength The number of bytes available in the buffer tag. +* \return Indicates any error encountered. */ +VnError VnSpi_parseUserTag( + const char* response, + char* tag, + size_t tagLength); + +/** \brief Parses a response from reading the Model Number register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] productName The register's Product Name field. +* \param[in] productNameLength The number of bytes available in the buffer productName. +* \return Indicates any error encountered. */ +VnError VnSpi_parseModelNumber( + const char* response, + char* productName, + size_t productNameLength); + +/** \brief Parses a response from reading the Hardware Revision register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] revision The register's Revision field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseHardwareRevision( + const char* response, + uint32_t* revision); + +/** \brief Parses a response from reading the Serial Number register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] serialNum The register's SerialNum field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseSerialNumber( + const char* response, + uint32_t* serialNum); + +/** \brief Parses a response from reading the Firmware Version register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] firmwareVersion The register's Firmware Version field. +* \param[in] firmwareVersionLength The number of bytes available in the buffer firmwareVersion. +* \return Indicates any error encountered. */ +VnError VnSpi_parseFirmwareVersion( + const char* response, + char* firmwareVersion, + size_t firmwareVersionLength); + +/** \brief Parses a response from reading the Serial Baud Rate register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] baudrate The register's Baud Rate field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseSerialBaudRate( + const char* response, + uint32_t* baudrate); + +/** \brief Parses a response from reading the Async Data Output Type register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] ador The register's ADOR field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseAsyncDataOutputType( + const char* response, + uint32_t* ador); + +/** \brief Parses a response from reading the Async Data Output Frequency register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] adof The register's ADOF field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseAsyncDataOutputFrequency( + const char* response, + uint32_t* adof); + +/** \brief Parses a response from reading the Yaw Pitch Roll register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] yawPitchRoll The register's YawPitchRoll field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseYawPitchRoll( + const char* response, + vec3f* yawPitchRoll); + +/** \brief Parses a response from reading the Attitude Quaternion register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] quat The register's Quat field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseAttitudeQuaternion( + const char* response, + vec4f* quat); + +/** \brief Parses a response from reading the Quaternion, Magnetic, Acceleration and Angular Rates register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] quat The register's Quat field. +* \param[out] mag The register's Mag field. +* \param[out] accel The register's Accel field. +* \param[out] gyro The register's Gyro field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseQuaternionMagneticAccelerationAndAngularRates( + const char* response, + vec4f* quat, + vec3f* mag, + vec3f* accel, + vec3f* gyro); + +/** \brief Parses a response from reading the Magnetic Measurements register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] mag The register's Mag field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseMagneticMeasurements( + const char* response, + vec3f* mag); + +/** \brief Parses a response from reading the Acceleration Measurements register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] accel The register's Accel field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseAccelerationMeasurements( + const char* response, + vec3f* accel); + +/** \brief Parses a response from reading the Angular Rate Measurements register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] gyro The register's Gyro field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseAngularRateMeasurements( + const char* response, + vec3f* gyro); + +/** \brief Parses a response from reading the Magnetic, Acceleration and Angular Rates register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] mag The register's Mag field. +* \param[out] accel The register's Accel field. +* \param[out] gyro The register's Gyro field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseMagneticAccelerationAndAngularRates( + const char* response, + vec3f* mag, + vec3f* accel, + vec3f* gyro); + +/** \brief Parses a response from reading the Magnetic and Gravity Reference Vectors register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] magRef The register's MagRef field. +* \param[out] accRef The register's AccRef field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseMagneticAndGravityReferenceVectors( + const char* response, + vec3f* magRef, + vec3f* accRef); + +/** \brief Parses a response from reading the Filter Measurements Variance Parameters register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] angularWalkVariance The register's Angular Walk Variance field. +* \param[out] angularRateVariance The register's Angular Rate Variance field. +* \param[out] magneticVariance The register's Magnetic Variance field. +* \param[out] accelerationVariance The register's Acceleration Variance field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseFilterMeasurementsVarianceParameters( + const char* response, + float* angularWalkVariance, + vec3f* angularRateVariance, + vec3f* magneticVariance, + vec3f* accelerationVariance); + +/** \brief Parses a response from reading the Magnetometer Compensation register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] c The register's C field. +* \param[out] b The register's B field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseMagnetometerCompensation( + const char* response, + mat3f* c, + vec3f* b); + +/** \brief Parses a response from reading the Filter Active Tuning Parameters register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] magneticDisturbanceGain The register's Magnetic Disturbance Gain field. +* \param[out] accelerationDisturbanceGain The register's Acceleration Disturbance Gain field. +* \param[out] magneticDisturbanceMemory The register's Magnetic Disturbance Memory field. +* \param[out] accelerationDisturbanceMemory The register's Acceleration Disturbance Memory field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseFilterActiveTuningParameters( + const char* response, + float* magneticDisturbanceGain, + float* accelerationDisturbanceGain, + float* magneticDisturbanceMemory, + float* accelerationDisturbanceMemory); + +/** \brief Parses a response from reading the Acceleration Compensation register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] c The register's C field. +* \param[out] b The register's B field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseAccelerationCompensation( + const char* response, + mat3f* c, + vec3f* b); + +/** \brief Parses a response from reading the Reference Frame Rotation register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] c The register's C field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseReferenceFrameRotation( + const char* response, + mat3f* c); + +/** \brief Parses a response from reading the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] yawPitchRoll The register's YawPitchRoll field. +* \param[out] mag The register's Mag field. +* \param[out] accel The register's Accel field. +* \param[out] gyro The register's Gyro field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseYawPitchRollMagneticAccelerationAndAngularRates( + const char* response, + vec3f* yawPitchRoll, + vec3f* mag, + vec3f* accel, + vec3f* gyro); + +/** \brief Parses a response from reading the Communication Protocol Control register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] serialCount The register's SerialCount field. +* \param[out] serialStatus The register's SerialStatus field. +* \param[out] spiCount The register's SPICount field. +* \param[out] spiStatus The register's SPIStatus field. +* \param[out] serialChecksum The register's SerialChecksum field. +* \param[out] spiChecksum The register's SPIChecksum field. +* \param[out] errorMode The register's ErrorMode field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseCommunicationProtocolControl( + const char* response, + uint8_t* serialCount, + uint8_t* serialStatus, + uint8_t* spiCount, + uint8_t* spiStatus, + uint8_t* serialChecksum, + uint8_t* spiChecksum, + uint8_t* errorMode); + +/** \brief Parses a response from reading the Synchronization Control register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] syncInMode The register's SyncInMode field. +* \param[out] syncInEdge The register's SyncInEdge field. +* \param[out] syncInSkipFactor The register's SyncInSkipFactor field. +* \param[out] reserved1 The register's RESERVED1 field. +* \param[out] syncOutMode The register's SyncOutMode field. +* \param[out] syncOutPolarity The register's SyncOutPolarity field. +* \param[out] syncOutSkipFactor The register's SyncOutSkipFactor field. +* \param[out] syncOutPulseWidth The register's SyncOutPulseWidth field. +* \param[out] reserved2 The register's RESERVED2 field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseSynchronizationControl( + const char* response, + uint8_t* syncInMode, + uint8_t* syncInEdge, + uint16_t* syncInSkipFactor, + uint32_t* reserved1, + uint8_t* syncOutMode, + uint8_t* syncOutPolarity, + uint16_t* syncOutSkipFactor, + uint32_t* syncOutPulseWidth, + uint32_t* reserved2); + +/** \brief Parses a response from reading the Synchronization Status register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] syncInCount The register's SyncInCount field. +* \param[out] syncInTime The register's SyncInTime field. +* \param[out] syncOutCount The register's SyncOutCount field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseSynchronizationStatus( + const char* response, + uint32_t* syncInCount, + uint32_t* syncInTime, + uint32_t* syncOutCount); + +/** \brief Parses a response from reading the Filter Basic Control register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] magMode The register's MagMode field. +* \param[out] extMagMode The register's ExtMagMode field. +* \param[out] extAccMode The register's ExtAccMode field. +* \param[out] extGyroMode The register's ExtGyroMode field. +* \param[out] gyroLimit The register's GyroLimit field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseFilterBasicControl( + const char* response, + uint8_t* magMode, + uint8_t* extMagMode, + uint8_t* extAccMode, + uint8_t* extGyroMode, + vec3f* gyroLimit); + +/** \brief Parses a response from reading the VPE Basic Control register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] enable The register's Enable field. +* \param[out] headingMode The register's HeadingMode field. +* \param[out] filteringMode The register's FilteringMode field. +* \param[out] tuningMode The register's TuningMode field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseVpeBasicControl( + const char* response, + uint8_t* enable, + uint8_t* headingMode, + uint8_t* filteringMode, + uint8_t* tuningMode); + +/** \brief Parses a response from reading the VPE Magnetometer Basic Tuning register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] baseTuning The register's BaseTuning field. +* \param[out] adaptiveTuning The register's AdaptiveTuning field. +* \param[out] adaptiveFiltering The register's AdaptiveFiltering field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseVpeMagnetometerBasicTuning( + const char* response, + vec3f* baseTuning, + vec3f* adaptiveTuning, + vec3f* adaptiveFiltering); + +/** \brief Parses a response from reading the VPE Magnetometer Advanced Tuning register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] minFiltering The register's MinFiltering field. +* \param[out] maxFiltering The register's MaxFiltering field. +* \param[out] maxAdaptRate The register's MaxAdaptRate field. +* \param[out] disturbanceWindow The register's DisturbanceWindow field. +* \param[out] maxTuning The register's MaxTuning field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseVpeMagnetometerAdvancedTuning( + const char* response, + vec3f* minFiltering, + vec3f* maxFiltering, + float* maxAdaptRate, + float* disturbanceWindow, + float* maxTuning); + +/** \brief Parses a response from reading the VPE Accelerometer Basic Tuning register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] baseTuning The register's BaseTuning field. +* \param[out] adaptiveTuning The register's AdaptiveTuning field. +* \param[out] adaptiveFiltering The register's AdaptiveFiltering field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseVpeAccelerometerBasicTuning( + const char* response, + vec3f* baseTuning, + vec3f* adaptiveTuning, + vec3f* adaptiveFiltering); + +/** \brief Parses a response from reading the VPE Accelerometer Advanced Tuning register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] minFiltering The register's MinFiltering field. +* \param[out] maxFiltering The register's MaxFiltering field. +* \param[out] maxAdaptRate The register's MaxAdaptRate field. +* \param[out] disturbanceWindow The register's DisturbanceWindow field. +* \param[out] maxTuning The register's MaxTuning field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseVpeAccelerometerAdvancedTuning( + const char* response, + vec3f* minFiltering, + vec3f* maxFiltering, + float* maxAdaptRate, + float* disturbanceWindow, + float* maxTuning); + +/** \brief Parses a response from reading the VPE Gyro Basic Tuning register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] angularWalkVariance The register's AngularWalkVariance field. +* \param[out] baseTuning The register's BaseTuning field. +* \param[out] adaptiveTuning The register's AdaptiveTuning field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseVpeGyroBasicTuning( + const char* response, + vec3f* angularWalkVariance, + vec3f* baseTuning, + vec3f* adaptiveTuning); + +/** \brief Parses a response from reading the Filter Startup Gyro Bias register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] bias The register's Bias field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseFilterStartupGyroBias( + const char* response, + vec3f* bias); + +/** \brief Parses a response from reading the Magnetometer Calibration Control register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] hsiMode The register's HSIMode field. +* \param[out] hsiOutput The register's HSIOutput field. +* \param[out] convergeRate The register's ConvergeRate field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseMagnetometerCalibrationControl( + const char* response, + uint8_t* hsiMode, + uint8_t* hsiOutput, + uint8_t* convergeRate); + +/** \brief Parses a response from reading the Calculated Magnetometer Calibration register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] c The register's C field. +* \param[out] b The register's B field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseCalculatedMagnetometerCalibration( + const char* response, + mat3f* c, + vec3f* b); + +/** \brief Parses a response from reading the Indoor Heading Mode Control register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] maxRateError The register's Max Rate Error field. +* \param[out] reserved1 The register's Reserved1 field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseIndoorHeadingModeControl( + const char* response, + float* maxRateError, + uint8_t* reserved1); + +/** \brief Parses a response from reading the Velocity Compensation Measurement register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] velocity The register's Velocity field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseVelocityCompensationMeasurement( + const char* response, + vec3f* velocity); + +/** \brief Parses a response from reading the Velocity Compensation Control register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] mode The register's Mode field. +* \param[out] velocityTuning The register's VelocityTuning field. +* \param[out] rateTuning The register's RateTuning field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseVelocityCompensationControl( + const char* response, + uint8_t* mode, + float* velocityTuning, + float* rateTuning); + +/** \brief Parses a response from reading the Velocity Compensation Status register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] x The register's x field. +* \param[out] xDot The register's xDot field. +* \param[out] accelOffset The register's accelOffset field. +* \param[out] omega The register's omega field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseVelocityCompensationStatus( + const char* response, + float* x, + float* xDot, + vec3f* accelOffset, + vec3f* omega); + +/** \brief Parses a response from reading the IMU Measurements register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] mag The register's Mag field. +* \param[out] accel The register's Accel field. +* \param[out] gyro The register's Gyro field. +* \param[out] temp The register's Temp field. +* \param[out] pressure The register's Pressure field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseImuMeasurements( + const char* response, + vec3f* mag, + vec3f* accel, + vec3f* gyro, + float* temp, + float* pressure); + +/** \brief Parses a response from reading the GPS Configuration register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] mode The register's Mode field. +* \param[out] ppsSource The register's PpsSource field. +* \param[out] reserved1 The register's Reserved1 field. +* \param[out] reserved2 The register's Reserved2 field. +* \param[out] reserved3 The register's Reserved3 field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseGpsConfiguration( + const char* response, + uint8_t* mode, + uint8_t* ppsSource, + uint8_t* reserved1, + uint8_t* reserved2, + uint8_t* reserved3); + +/** \brief Parses a response from reading the GPS Antenna Offset register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] position The register's Position field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseGpsAntennaOffset( + const char* response, + vec3f* position); + +/** \brief Parses a response from reading the GPS Solution - LLA register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] time The register's Time field. +* \param[out] week The register's Week field. +* \param[out] gpsFix The register's GpsFix field. +* \param[out] numSats The register's NumSats field. +* \param[out] lla The register's Lla field. +* \param[out] nedVel The register's NedVel field. +* \param[out] nedAcc The register's NedAcc field. +* \param[out] speedAcc The register's SpeedAcc field. +* \param[out] timeAcc The register's TimeAcc field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseGpsSolutionLla( + const char* response, + double* time, + uint16_t* week, + uint8_t* gpsFix, + uint8_t* numSats, + vec3d* lla, + vec3f* nedVel, + vec3f* nedAcc, + float* speedAcc, + float* timeAcc); + +/** \brief Parses a response from reading the GPS Solution - ECEF register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] tow The register's Tow field. +* \param[out] week The register's Week field. +* \param[out] gpsFix The register's GpsFix field. +* \param[out] numSats The register's NumSats field. +* \param[out] position The register's Position field. +* \param[out] velocity The register's Velocity field. +* \param[out] posAcc The register's PosAcc field. +* \param[out] speedAcc The register's SpeedAcc field. +* \param[out] timeAcc The register's TimeAcc field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseGpsSolutionEcef( + const char* response, + double* tow, + uint16_t* week, + uint8_t* gpsFix, + uint8_t* numSats, + vec3d* position, + vec3f* velocity, + vec3f* posAcc, + float* speedAcc, + float* timeAcc); + +/** \brief Parses a response from reading the INS Solution - LLA register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] time The register's Time field. +* \param[out] week The register's Week field. +* \param[out] status The register's Status field. +* \param[out] yawPitchRoll The register's YawPitchRoll field. +* \param[out] position The register's Position field. +* \param[out] nedVel The register's NedVel field. +* \param[out] attUncertainty The register's AttUncertainty field. +* \param[out] posUncertainty The register's PosUncertainty field. +* \param[out] velUncertainty The register's VelUncertainty field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseInsSolutionLla( + const char* response, + double* time, + uint16_t* week, + uint16_t* status, + vec3f* yawPitchRoll, + vec3d* position, + vec3f* nedVel, + float* attUncertainty, + float* posUncertainty, + float* velUncertainty); + +/** \brief Parses a response from reading the INS Solution - ECEF register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] time The register's Time field. +* \param[out] week The register's Week field. +* \param[out] status The register's Status field. +* \param[out] yawPitchRoll The register's YawPitchRoll field. +* \param[out] position The register's Position field. +* \param[out] velocity The register's Velocity field. +* \param[out] attUncertainty The register's AttUncertainty field. +* \param[out] posUncertainty The register's PosUncertainty field. +* \param[out] velUncertainty The register's VelUncertainty field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseInsSolutionEcef( + const char* response, + double* time, + uint16_t* week, + uint16_t* status, + vec3f* yawPitchRoll, + vec3d* position, + vec3f* velocity, + float* attUncertainty, + float* posUncertainty, + float* velUncertainty); + +/** \brief Parses a response from reading the INS Basic Configuration register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] scenario The register's Scenario field. +* \param[out] ahrsAiding The register's AhrsAiding field. +* \param[out] estBaseline The register's EstBaseline field. +* \param[out] resv2 The register's Resv2 field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseInsBasicConfiguration( + const char* response, + uint8_t* scenario, + uint8_t* ahrsAiding, + uint8_t* estBaseline, + uint8_t* resv2); + +/** \brief Parses a response from reading the INS Advanced Configuration register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] useMag The register's UseMag field. +* \param[out] usePres The register's UsePres field. +* \param[out] posAtt The register's PosAtt field. +* \param[out] velAtt The register's VelAtt field. +* \param[out] velBias The register's VelBias field. +* \param[out] useFoam The register's UseFoam field. +* \param[out] gpsCovType The register's GPSCovType field. +* \param[out] velCount The register's VelCount field. +* \param[out] velInit The register's VelInit field. +* \param[out] moveOrigin The register's MoveOrigin field. +* \param[out] gpsTimeout The register's GPSTimeout field. +* \param[out] deltaLimitPos The register's DeltaLimitPos field. +* \param[out] deltaLimitVel The register's DeltaLimitVel field. +* \param[out] minPosUncertainty The register's MinPosUncertainty field. +* \param[out] minVelUncertainty The register's MinVelUncertainty field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseInsAdvancedConfiguration( + const char* response, + uint8_t* useMag, + uint8_t* usePres, + uint8_t* posAtt, + uint8_t* velAtt, + uint8_t* velBias, + uint8_t* useFoam, + uint8_t* gpsCovType, + uint8_t* velCount, + float* velInit, + float* moveOrigin, + float* gpsTimeout, + float* deltaLimitPos, + float* deltaLimitVel, + float* minPosUncertainty, + float* minVelUncertainty); + +/** \brief Parses a response from reading the INS State - LLA register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] yawPitchRoll The register's YawPitchRoll field. +* \param[out] position The register's Position field. +* \param[out] velocity The register's Velocity field. +* \param[out] accel The register's Accel field. +* \param[out] angularRate The register's AngularRate field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseInsStateLla( + const char* response, + vec3f* yawPitchRoll, + vec3d* position, + vec3f* velocity, + vec3f* accel, + vec3f* angularRate); + +/** \brief Parses a response from reading the INS State - ECEF register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] yawPitchRoll The register's YawPitchRoll field. +* \param[out] position The register's Position field. +* \param[out] velocity The register's Velocity field. +* \param[out] accel The register's Accel field. +* \param[out] angularRate The register's AngularRate field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseInsStateEcef( + const char* response, + vec3f* yawPitchRoll, + vec3d* position, + vec3f* velocity, + vec3f* accel, + vec3f* angularRate); + +/** \brief Parses a response from reading the Startup Filter Bias Estimate register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] gyroBias The register's GyroBias field. +* \param[out] accelBias The register's AccelBias field. +* \param[out] pressureBias The register's PressureBias field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseStartupFilterBiasEstimate( + const char* response, + vec3f* gyroBias, + vec3f* accelBias, + float* pressureBias); + +/** \brief Parses a response from reading the Delta Theta and Delta Velocity register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] deltaTime The register's DeltaTime field. +* \param[out] deltaTheta The register's DeltaTheta field. +* \param[out] deltaVelocity The register's DeltaVelocity field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseDeltaThetaAndDeltaVelocity( + const char* response, + float* deltaTime, + vec3f* deltaTheta, + vec3f* deltaVelocity); + +/** \brief Parses a response from reading the Delta Theta and Delta Velocity Configuration register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] integrationFrame The register's IntegrationFrame field. +* \param[out] gyroCompensation The register's GyroCompensation field. +* \param[out] accelCompensation The register's AccelCompensation field. +* \param[out] reserved1 The register's Reserved1 field. +* \param[out] reserved2 The register's Reserved2 field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseDeltaThetaAndDeltaVelocityConfiguration( + const char* response, + uint8_t* integrationFrame, + uint8_t* gyroCompensation, + uint8_t* accelCompensation, + uint8_t* reserved1, + uint16_t* reserved2); + +/** \brief Parses a response from reading the Reference Vector Configuration register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] useMagModel The register's UseMagModel field. +* \param[out] useGravityModel The register's UseGravityModel field. +* \param[out] resv1 The register's Resv1 field. +* \param[out] resv2 The register's Resv2 field. +* \param[out] recalcThreshold The register's RecalcThreshold field. +* \param[out] year The register's Year field. +* \param[out] position The register's Position field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseReferenceVectorConfiguration( + const char* response, + uint8_t* useMagModel, + uint8_t* useGravityModel, + uint8_t* resv1, + uint8_t* resv2, + uint32_t* recalcThreshold, + float* year, + vec3d* position); + +/** \brief Parses a response from reading the Gyro Compensation register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] c The register's C field. +* \param[out] b The register's B field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseGyroCompensation( + const char* response, + mat3f* c, + vec3f* b); + +/** \brief Parses a response from reading the IMU Filtering Configuration register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] magWindowSize The register's MagWindowSize field. +* \param[out] accelWindowSize The register's AccelWindowSize field. +* \param[out] gyroWindowSize The register's GyroWindowSize field. +* \param[out] tempWindowSize The register's TempWindowSize field. +* \param[out] presWindowSize The register's PresWindowSize field. +* \param[out] magFilterMode The register's MagFilterMode field. +* \param[out] accelFilterMode The register's AccelFilterMode field. +* \param[out] gyroFilterMode The register's GyroFilterMode field. +* \param[out] tempFilterMode The register's TempFilterMode field. +* \param[out] presFilterMode The register's PresFilterMode field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseImuFilteringConfiguration( + const char* response, + uint16_t* magWindowSize, + uint16_t* accelWindowSize, + uint16_t* gyroWindowSize, + uint16_t* tempWindowSize, + uint16_t* presWindowSize, + uint8_t* magFilterMode, + uint8_t* accelFilterMode, + uint8_t* gyroFilterMode, + uint8_t* tempFilterMode, + uint8_t* presFilterMode); + +/** \brief Parses a response from reading the GPS Compass Baseline register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] position The register's Position field. +* \param[out] uncertainty The register's Uncertainty field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseGpsCompassBaseline( + const char* response, + vec3f* position, + vec3f* uncertainty); + +/** \brief Parses a response from reading the GPS Compass Estimated Baseline register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] estBaselineUsed The register's EstBaselineUsed field. +* \param[out] resv The register's Resv field. +* \param[out] numMeas The register's NumMeas field. +* \param[out] position The register's Position field. +* \param[out] uncertainty The register's Uncertainty field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseGpsCompassEstimatedBaseline( + const char* response, + uint8_t* estBaselineUsed, + uint8_t* resv, + uint16_t* numMeas, + vec3f* position, + vec3f* uncertainty); + +/** \brief Parses a response from reading the IMU Rate Configuration register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] imuRate The register's imuRate field. +* \param[out] navDivisor The register's NavDivisor field. +* \param[out] filterTargetRate The register's filterTargetRate field. +* \param[out] filterMinRate The register's filterMinRate field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseImuRateConfiguration( + const char* response, + uint16_t* imuRate, + uint16_t* navDivisor, + float* filterTargetRate, + float* filterMinRate); + +/** \brief Parses a response from reading the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] yawPitchRoll The register's YawPitchRoll field. +* \param[out] bodyAccel The register's BodyAccel field. +* \param[out] gyro The register's Gyro field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseYawPitchRollTrueBodyAccelerationAndAngularRates( + const char* response, + vec3f* yawPitchRoll, + vec3f* bodyAccel, + vec3f* gyro); + +/** \brief Parses a response from reading the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register using the SPI protocol. +* +* \param[in] response Pointer to the buffer containing the response from the VectorNav sensor, including the leading 0x00 byte. +* \param[out] yawPitchRoll The register's YawPitchRoll field. +* \param[out] inertialAccel The register's InertialAccel field. +* \param[out] gyro The register's Gyro field. +* \return Indicates any error encountered. */ +VnError VnSpi_parseYawPitchRollTrueInertialAccelerationAndAngularRates( + const char* response, + vec3f* yawPitchRoll, + vec3f* inertialAccel, + vec3f* gyro); + +/** \defgroup spi_genread_functions SPI Generate Read Functions +* \brief This set of functions will generate command strings for reading +* registers on VectorNav sensors using the SPI protocol. +* +* These functions take the form of +* VnError VnSpi_genReadXXX(char *buffer, size_t* size, size_t desiredLength, size_t* responseSize) +* where XXX is replaced with the name of the register, buffer is provided by +* the user to be filled with the generated command, size is the number of +* bytes available in the provided buffer and will contain the number of bytes of the command +* to send, desiredLength is the total number of bytes to pad with 0x00 should +* the total constructed length of the command be less than the desiredLength, +* responseSize is the number of bytes to read during the SPI transaction to get this +* command's response. +* +* \{ */ + +/** \brief Generates a command to read the User Tag register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadUserTag( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Model Number register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadModelNumber( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Hardware Revision register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadHardwareRevision( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Serial Number register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadSerialNumber( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Firmware Version register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadFirmwareVersion( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Serial Baud Rate register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadSerialBaudRate( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Async Data Output Type register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadAsyncDataOutputType( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Async Data Output Frequency register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadAsyncDataOutputFrequency( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Yaw Pitch Roll register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadYawPitchRoll( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Attitude Quaternion register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadAttitudeQuaternion( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Quaternion, Magnetic, Acceleration and Angular Rates register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadQuaternionMagneticAccelerationAndAngularRates( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Magnetic Measurements register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadMagneticMeasurements( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Acceleration Measurements register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadAccelerationMeasurements( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Angular Rate Measurements register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadAngularRateMeasurements( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Magnetic, Acceleration and Angular Rates register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadMagneticAccelerationAndAngularRates( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Magnetic and Gravity Reference Vectors register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadMagneticAndGravityReferenceVectors( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Magnetometer Compensation register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadMagnetometerCompensation( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Acceleration Compensation register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadAccelerationCompensation( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Reference Frame Rotation register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadReferenceFrameRotation( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadYawPitchRollMagneticAccelerationAndAngularRates( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Communication Protocol Control register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadCommunicationProtocolControl( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Synchronization Control register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadSynchronizationControl( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Synchronization Status register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadSynchronizationStatus( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the VPE Basic Control register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadVpeBasicControl( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the VPE Magnetometer Basic Tuning register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadVpeMagnetometerBasicTuning( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the VPE Accelerometer Basic Tuning register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadVpeAccelerometerBasicTuning( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Magnetometer Calibration Control register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadMagnetometerCalibrationControl( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Calculated Magnetometer Calibration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadCalculatedMagnetometerCalibration( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Velocity Compensation Measurement register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadVelocityCompensationMeasurement( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Velocity Compensation Control register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadVelocityCompensationControl( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the IMU Measurements register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadImuMeasurements( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the GPS Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadGpsConfiguration( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the GPS Antenna Offset register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadGpsAntennaOffset( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the GPS Solution - LLA register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadGpsSolutionLla( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the GPS Solution - ECEF register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadGpsSolutionEcef( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the INS Solution - LLA register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadInsSolutionLla( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the INS Solution - ECEF register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadInsSolutionEcef( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the INS Basic Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadInsBasicConfiguration( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the INS Basic Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadInsBasicConfiguration( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the INS State - LLA register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadInsStateLla( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the INS State - ECEF register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadInsStateEcef( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Startup Filter Bias Estimate register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadStartupFilterBiasEstimate( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Delta Theta and Delta Velocity register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadDeltaThetaAndDeltaVelocity( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Delta Theta and Delta Velocity Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadDeltaThetaAndDeltaVelocityConfiguration( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Reference Vector Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadReferenceVectorConfiguration( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Gyro Compensation register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadGyroCompensation( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the IMU Filtering Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadImuFilteringConfiguration( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the GPS Compass Baseline register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadGpsCompassBaseline( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the GPS Compass Estimated Baseline register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadGpsCompassEstimatedBaseline( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadYawPitchRollTrueBodyAccelerationAndAngularRates( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/** \brief Generates a command to read the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \return Indicates any errors encountered. */ +VnError VnSpi_genReadYawPitchRollTrueInertialAccelerationAndAngularRates( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize); + +/* \}*/ + +/** \defgroup spi_genwrite_functions SPI Generate Write Functions +* \brief This set of functions will generate command strings for writing to +* registers on VectorNav sensors using the SPI protocol. +* +* These functions take the form shown below. XXX is replaced by the name +* of the register, buffer is provided by the user to be filled with the +* generated command, size is the number of bytes available in the provided +* buffer and will contain the number of bytes of the command to send, desiredLength +* is the total number of bytes to pad with 0x00 should the total constructed length +* of the command be less than the desired length, and [Variable argument list] +* varies with the specified register being written to. +* +* \code + * VnError VnSpi_genWriteXXX( + * char *buffer, + * size_t* size, + * size_t desiredLength, + * size_t* responseSize, + * [Variable argument list]); + * \endcode + * + * \{ */ + +/** \brief Generates a command to write the User Tag register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] tag The register's Tag field. This should be a null-terminated string. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteUserTag( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + char* tag); + +/** \brief Generates a command to write the Serial Baud Rate register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] baudrate The register's Baud Rate field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteSerialBaudRate( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint32_t baudrate); + +/** \brief Generates a command to write the Serial Baud Rate register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] baudrate The register's Baud Rate field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteSerialBaudRateWithOptions( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint32_t baudrate); + +/** \brief Generates a command to write the Async Data Output Type register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] ador The register's ADOR field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteAsyncDataOutputType( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint32_t ador); + +/** \brief Generates a command to write the Async Data Output Type register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] ador The register's ADOR field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteAsyncDataOutputTypeWithOptions( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint32_t ador); + +/** \brief Generates a command to write the Async Data Output Frequency register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] adof The register's ADOF field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteAsyncDataOutputFrequency( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint32_t adof); + +/** \brief Generates a command to write the Async Data Output Frequency register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] adof The register's ADOF field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteAsyncDataOutputFrequencyWithOptions( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint32_t adof); + +/** \brief Generates a command to write the Magnetic and Gravity Reference Vectors register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] magRef The register's MagRef field. + * \param[in] accRef The register's AccRef field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteMagneticAndGravityReferenceVectors( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + vec3f magRef, + vec3f accRef); + +/** \brief Generates a command to write the Magnetometer Compensation register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] c The register's C field. + * \param[in] b The register's B field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteMagnetometerCompensation( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + mat3f c, + vec3f b); + +/** \brief Generates a command to write the Acceleration Compensation register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] c The register's C field. + * \param[in] b The register's B field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteAccelerationCompensation( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + mat3f c, + vec3f b); + +/** \brief Generates a command to write the Reference Frame Rotation register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] c The register's C field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteReferenceFrameRotation( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + mat3f c); + +/** \brief Generates a command to write the Communication Protocol Control register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] serialCount The register's SerialCount field. + * \param[in] serialStatus The register's SerialStatus field. + * \param[in] spiCount The register's SPICount field. + * \param[in] spiStatus The register's SPIStatus field. + * \param[in] serialChecksum The register's SerialChecksum field. + * \param[in] spiChecksum The register's SPIChecksum field. + * \param[in] errorMode The register's ErrorMode field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteCommunicationProtocolControl( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint8_t serialCount, + uint8_t serialStatus, + uint8_t spiCount, + uint8_t spiStatus, + uint8_t serialChecksum, + uint8_t spiChecksum, + uint8_t errorMode); + +/** \brief Generates a command to write the Synchronization Control register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] syncInMode The register's SyncInMode field. + * \param[in] syncInEdge The register's SyncInEdge field. + * \param[in] syncInSkipFactor The register's SyncInSkipFactor field. + * \param[in] reserved1 The register's RESERVED1 field. + * \param[in] syncOutMode The register's SyncOutMode field. + * \param[in] syncOutPolarity The register's SyncOutPolarity field. + * \param[in] syncOutSkipFactor The register's SyncOutSkipFactor field. + * \param[in] syncOutPulseWidth The register's SyncOutPulseWidth field. + * \param[in] reserved2 The register's RESERVED2 field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteSynchronizationControl( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint8_t syncInMode, + uint8_t syncInEdge, + uint16_t syncInSkipFactor, + uint32_t reserved1, + uint8_t syncOutMode, + uint8_t syncOutPolarity, + uint16_t syncOutSkipFactor, + uint32_t syncOutPulseWidth, + uint32_t reserved2); + +/** \brief Generates a command to write the Synchronization Status register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] syncInCount The register's SyncInCount field. + * \param[in] syncInTime The register's SyncInTime field. + * \param[in] syncOutCount The register's SyncOutCount field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteSynchronizationStatus( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint32_t syncInCount, + uint32_t syncInTime, + uint32_t syncOutCount); + +/** \brief Generates a command to write the VPE Basic Control register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] enable The register's Enable field. + * \param[in] headingMode The register's HeadingMode field. + * \param[in] filteringMode The register's FilteringMode field. + * \param[in] tuningMode The register's TuningMode field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteVpeBasicControl( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint8_t enable, + uint8_t headingMode, + uint8_t filteringMode, + uint8_t tuningMode); + +/** \brief Generates a command to write the VPE Magnetometer Basic Tuning register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] baseTuning The register's BaseTuning field. + * \param[in] adaptiveTuning The register's AdaptiveTuning field. + * \param[in] adaptiveFiltering The register's AdaptiveFiltering field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteVpeMagnetometerBasicTuning( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + vec3f baseTuning, + vec3f adaptiveTuning, + vec3f adaptiveFiltering); + +/** \brief Generates a command to write the VPE Accelerometer Basic Tuning register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] baseTuning The register's BaseTuning field. + * \param[in] adaptiveTuning The register's AdaptiveTuning field. + * \param[in] adaptiveFiltering The register's AdaptiveFiltering field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteVpeAccelerometerBasicTuning( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + vec3f baseTuning, + vec3f adaptiveTuning, + vec3f adaptiveFiltering); + +/** \brief Generates a command to write the Magnetometer Calibration Control register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] hsiMode The register's HSIMode field. + * \param[in] hsiOutput The register's HSIOutput field. + * \param[in] convergeRate The register's ConvergeRate field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteMagnetometerCalibrationControl( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint8_t hsiMode, + uint8_t hsiOutput, + uint8_t convergeRate); + +/** \brief Generates a command to write the Velocity Compensation Measurement register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] velocity The register's Velocity field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteVelocityCompensationMeasurement( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + vec3f velocity); + +/** \brief Generates a command to write the Velocity Compensation Control register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] mode The register's Mode field. + * \param[in] velocityTuning The register's VelocityTuning field. + * \param[in] rateTuning The register's RateTuning field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteVelocityCompensationControl( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint8_t mode, + float velocityTuning, + float rateTuning); + +/** \brief Generates a command to write the GPS Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] mode The register's Mode field. + * \param[in] ppsSource The register's PpsSource field. + * \param[in] reserved1 The register's Reserved1 field. + * \param[in] reserved2 The register's Reserved2 field. + * \param[in] reserved3 The register's Reserved3 field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteGpsConfiguration( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint8_t mode, + uint8_t ppsSource, + uint8_t reserved1, + uint8_t reserved2, + uint8_t reserved3); + +/** \brief Generates a command to write the GPS Antenna Offset register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] position The register's Position field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteGpsAntennaOffset( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + vec3f position); + +/** \brief Generates a command to write the INS Basic Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] scenario The register's Scenario field. + * \param[in] ahrsAiding The register's AhrsAiding field. + * \param[in] estBaseline The register's EstBaseline field. + * \param[in] resv2 The register's Resv2 field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteInsBasicConfiguration( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint8_t scenario, + uint8_t ahrsAiding, + uint8_t estBaseline, + uint8_t resv2); + +/** \brief Generates a command to write the Startup Filter Bias Estimate register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] gyroBias The register's GyroBias field. + * \param[in] accelBias The register's AccelBias field. + * \param[in] pressureBias The register's PressureBias field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteStartupFilterBiasEstimate( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + vec3f gyroBias, + vec3f accelBias, + float pressureBias); + +/** \brief Generates a command to write the Delta Theta and Delta Velocity Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] integrationFrame The register's IntegrationFrame field. + * \param[in] gyroCompensation The register's GyroCompensation field. + * \param[in] accelCompensation The register's AccelCompensation field. + * \param[in] reserved1 The register's Reserved1 field. + * \param[in] reserved2 The register's Reserved2 field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteDeltaThetaAndDeltaVelocityConfiguration( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint8_t integrationFrame, + uint8_t gyroCompensation, + uint8_t accelCompensation, + uint8_t reserved1, + uint16_t reserved2); + +/** \brief Generates a command to write the Reference Vector Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] useMagModel The register's UseMagModel field. + * \param[in] useGravityModel The register's UseGravityModel field. + * \param[in] resv1 The register's Resv1 field. + * \param[in] resv2 The register's Resv2 field. + * \param[in] recalcThreshold The register's RecalcThreshold field. + * \param[in] year The register's Year field. + * \param[in] position The register's Position field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteReferenceVectorConfiguration( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint8_t useMagModel, + uint8_t useGravityModel, + uint8_t resv1, + uint8_t resv2, + uint32_t recalcThreshold, + float year, + vec3d position); + +/** \brief Generates a command to write the Gyro Compensation register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] c The register's C field. + * \param[in] b The register's B field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteGyroCompensation( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + mat3f c, + vec3f b); + +/** \brief Generates a command to write the IMU Filtering Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] magWindowSize The register's MagWindowSize field. + * \param[in] accelWindowSize The register's AccelWindowSize field. + * \param[in] gyroWindowSize The register's GyroWindowSize field. + * \param[in] tempWindowSize The register's TempWindowSize field. + * \param[in] presWindowSize The register's PresWindowSize field. + * \param[in] magFilterMode The register's MagFilterMode field. + * \param[in] accelFilterMode The register's AccelFilterMode field. + * \param[in] gyroFilterMode The register's GyroFilterMode field. + * \param[in] tempFilterMode The register's TempFilterMode field. + * \param[in] presFilterMode The register's PresFilterMode field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteImuFilteringConfiguration( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint16_t magWindowSize, + uint16_t accelWindowSize, + uint16_t gyroWindowSize, + uint16_t tempWindowSize, + uint16_t presWindowSize, + uint8_t magFilterMode, + uint8_t accelFilterMode, + uint8_t gyroFilterMode, + uint8_t tempFilterMode, + uint8_t presFilterMode); + +/** \brief Generates a command to write the GPS Compass Baseline register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in/out] size Number of bytes available in the buffer. Will contain on output the number of bytes in the command to send. + * \param[in] desiredLength The total number of bytes to pad with 0x00 should + * the total constructed length of the command be less than the + * desiredLength. This is useful back-to-back command where the + * desiredLength will be the responseSize of the previous command sent. + * \param[out] responseSize The number of bytes to read during the SPI transaction to get this commands response. + * \param[in] position The register's Position field. + * \param[in] uncertainty The register's Uncertainty field. + * \return Indicates any errors encountered. */ +VnError VnSpi_genWriteGpsCompassBaseline( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + vec3f position, + vec3f uncertainty); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/protocol/upack.h b/src/drivers/ins/vectornav/libvnc/include/vn/protocol/upack.h new file mode 100644 index 0000000000..8360ceaa2c --- /dev/null +++ b/src/drivers/ins/vectornav/libvnc/include/vn/protocol/upack.h @@ -0,0 +1,3650 @@ +#ifndef VNUPACK_H_INCLUDED +#define VNUPACK_H_INCLUDED + +#include + +#include "vn/int.h" +#include "vn/bool.h" +#include "vn/enum.h" +#include "vn/error.h" +#include "vn/error_detection.h" +#include "vn/math/matrix.h" +#include "vn/math/vector.h" +#include "vn/protocol/common.h" + +#ifndef VNUART_PROTOCOL_BUFFER_SIZE + /** Default internal buffers size for handling received UART data. */ + #define VNUART_PROTOCOL_BUFFER_SIZE 256 +#endif + +#define VN_BINARY_START_CHAR 0xFA +#define VN_ASCII_START_CHAR '$' +#define VN_BOOTLOAD_START_CHAR 'V' + +#ifdef __cplusplus +extern "C" { +#endif + +/** \brief Defines signature of functions that can handle notification of error + * messages received from a VectorNav sensor. + */ +typedef void(*vnuart_sensor_error_received)(VnError error); + +/** \brief Structure representing a UART packet received from a VectorNav +* sensor. */ +typedef struct +{ + /** Current location for extracting binary data. */ + size_t curExtractLoc; + + /** Number of bytes in the packet. */ + size_t length; + + /** The packet data. */ + uint8_t* data; + +} VnUartPacket; + +/** \brief Initializes a VnUartPacket structure. +* +* \param[out] packet Pointer to VnUartPacket structure to initialize. +* \param[in] data Pointer to the packet data. +* \param[in] len The number of bytes in the data packet. */ +void VnUartPacket_initialize(VnUartPacket* packet, uint8_t* data, size_t len); + +/** \brief Initializes a VnUartPacket structure from a null terminated string. +* +* \param[out] packet Pointer to VnUartPacket structure to initialize. +* \param[in] data Pointer to the packet data. */ +void VnUartPacket_initializeFromStr(VnUartPacket* packet, char* data); + +/** \brief Performs data integrity check on the data packet. +* +* This will perform an 8-bit XOR checksum, a CRC16-CCITT CRC, or no checking +* depending on the provided data integrity in the packet. +* +* \param[in] packet The data packet to perform the check on. +* \return true if the packet passed the data integrity checks; otherwise +* false. +*/ +bool VnUartPacket_isValid(VnUartPacket *packet); + +/** \brief Indicates if the packet is an ASCII asynchronous message. + * + * \param[in] packet The data packet to check. + * \return true if the packet is an ASCII asynchronous message; + * otherwise false. + */ +bool VnUartPacket_isAsciiAsync(VnUartPacket *packet); + +/** \brief Indicates if the packet is a bootloader message. + * + * \param[in] packet The data packet to check. + * \return true if the packet is a bootloader message; + * otherwise false. + */ +bool VnUartPacket_isBootloader(VnUartPacket* packet); + + +/** \brief Indicates if the packet is a response to a message sent to the + * sensor. + * + * \param[in] packet The data packet to check. + * \return true if the packet is a response message; otherwise + * false. + */ +bool VnUartPacket_isResponse(VnUartPacket *packet); + +/** \brief Indicates if the packet is an ASCII error message. + * + * \return true if the packet is an error message; otherwise + * false. + */ +bool VnUartPacket_isError(VnUartPacket *packet); + +/** \brief Indicates if the packet is an ASCII error message. +* +* \return true if the packet is an error message; otherwise +* false. +*/ +bool VnUartPacket_isErrorRaw(uint8_t *packet); + +/** \brief Returns the type of packet. + * + * \param[in] packet The associated VnUartPacket. + * \return The packet type. */ +PacketType VnUartPacket_type(VnUartPacket *packet); + +/** \brief Computes and appends a checksum plus line termination characters to a command. +* +* \param[in] errorDetectionMode The error detection mode to use to compute and append the checksum. +* \param[in] packet The current packet without any checksum information. +* \param[in,out] length The current length of the provided packet. Will be updated with the +* final packet length. +* \return Any errors encountered. */ +VnError VnUartPacket_finalizeCommand(VnErrorDetectionMode errorDetectionMode, uint8_t *packet, size_t *length); + +/** \brief Returns the groups field of a binary packet. +* +* \param[in] packet The associated VnUartPacket. +* \return The groups present in the binary packet. */ +uint8_t VnUartPacket_groups(VnUartPacket* packet); + +/** \brief Returns the request group field of a binary packet at the specified index. +* +* \param[in] packet The associated VnUartPacket. +* \param[in] groupIndex The 0-based index of the requested group field. +* \return The group field. */ +uint16_t VnUartPacket_groupField(VnUartPacket* packet, size_t groupIndex); + +/** \brief Computes the expected number of bytes for a possible binary + * packet. + * + * This method requires that the group fields present and the complete + * collection of individual group description fields are present. + * + * \param[in] startOfPossibleBinaryPacket The start of the possible binary + * packet (i.e. the 0xFA character). + * + * \return The number of bytes expected for this binary packet. + */ +size_t VnUartPacket_computeBinaryPacketLength(uint8_t const *startOfPossibleBinaryPacket); + +/** \brief Computes the number of bytes expected for a binary group field. + * + * \param[in] groupType The group to calculate the total for. + * \param[in] groupField The flags for data types present. + * \return The number of bytes for this group. + */ +size_t VnUartPacket_computeNumOfBytesForBinaryGroupPayload(BinaryGroupType groupType, uint16_t groupField); + +/** \brief Determines if the packet is a compatible match for an expected + * binary output message type. + * + * \param[in] packet The packet to compare. + * \param[in] commonGroup The Common Group configuration. + * \param[in] timeGroup The Time Group configuration. + * \param[in] imuGroup The IMU Group configuration. + * \param[in] gpsGroup The GPS Group configuration. + * \param[in] attitudeGroup The Attitude Group configuration. + * \param[in] insGroup The INS Group configuration. + * \param[in] gps2Group The GPS2 Group configuration. + * \return true if the packet matches the expected group + * configuration; otherwise false. + */ +bool VnUartPacket_isCompatible( + VnUartPacket *packet, + CommonGroup commonGroup, + TimeGroup timeGroup, + ImuGroup imuGroup, + GpsGroup gpsGroup, + AttitudeGroup attitudeGroup, + InsGroup insGroup, + GpsGroup gps2Group); + +/** \defgroup uartPacketBinaryExtractors UART Binary Data Extractors + * \brief This group of methods are useful for extracting data from binary + * data packets. + * + * \{ */ + +/** \brief Extracts a uint8_t data type from a binary packet and advances + * the next extraction point appropriately. + * + * \param[in] packet The packet to extract the value from. + * \return The extracted value. + */ +uint8_t VnUartPacket_extractUint8(VnUartPacket *packet); + +/** \brief Extracts a int8_t data type from a binary packet and advances + * the next extraction point appropriately. + * + * \param[in] packet The packet to extract the value from. + * \return The extracted value. + */ +int8_t VnUartPacket_extractInt8(VnUartPacket *packet); + +/** \brief Extracts a uint16_t data type from a binary packet and advances + * the next extraction point appropriately. + * + * \param[in] packet The packet to extract the value from. + * \return The extracted value. + */ +uint16_t VnUartPacket_extractUint16(VnUartPacket *packet); + +/** \brief Extracts a uint32_t data type from a binary packet and advances + * the next extraction point appropriately. + * + * \param[in] packet The packet to extract the value from. + * \return The extracted value. + */ +uint32_t VnUartPacket_extractUint32(VnUartPacket *packet); + +/** \brief Extracts a uint64_t data type from a binary packet and advances + * the next extraction point appropriately. + * + * \param[in] packet The packet to extract the value from. + * \return The extracted value. + */ +uint64_t VnUartPacket_extractUint64(VnUartPacket *packet); + +/** \brief Extracts a float data type from a binary packet and advances the +* next extraction point appropriately. +* +* \param[in] packet The packet to extract the value from. +* \return The extracted value. */ +float VnUartPacket_extractFloat(VnUartPacket* packet); + +/** \brief Extracts a vec3f data type from a binary packet and advances + * the next extraction point appropriately. + * + * \param[in] packet The packet to extract the value from. + * \return The extracted value. + */ +vec3f VnUartPacket_extractVec3f(VnUartPacket *packet); + +/** \brief Extracts a vec3d data type from a binary packet and advances + * the next extraction point appropriately. + * + * \param[in] packet The packet to extract the value from. + * \return The extracted value. + */ +vec3d VnUartPacket_extractVec3d(VnUartPacket *packet); + +/** \brief Extracts a vec4f data type from a binary packet and advances + * the next extraction point appropriately. + * + * \param[in] packet The packet to extract the value from. + * \return The extracted value. + */ +vec4f VnUartPacket_extractVec4f(VnUartPacket *packet); + +/** \brief Extracts a mat3f data type from a binary packet and advances +* the next extraction point appropriately. +* +* \param[in] packet The packet to extract the value from. +* \return The extracted value. */ +mat3f VnUartPacket_extractMat3f(VnUartPacket *packet); + +/** \brief Extracts a GpsDop data type from a binary packet and advances +* the next extraction point appropriately. +* +* \param[in] packet The packet to extract the value from. +* \return The extracted value. */ +GpsDop VnUartPacket_extractGpsDop(VnUartPacket *packet); + + +/** \brief Extracts a TimeUtc data type from a binary packet and advances +* the next extraction point appropriately. +* +* \param[in] packet The packet to extract the value from. +* \return The extracted value. */ +TimeUtc VnUartPacket_extractTimeUtc(VnUartPacket *packet); + +/** \brief Extracts a TimeInfo data type from a binary packet and advances +* the next extraction point appropriately. +* +* \param[in] packet The packet to extract the value from. +* \return The extracted value. */ +TimeInfo VnUartPacket_extractTimeInfo(VnUartPacket *packet); +/** \} */ + +/** \brief Determines the type of ASCII asynchronous message this packet is. +* +* \param[in] packet The data packet to check. +* \return The asynchronous data type of the packet. +*/ +VnAsciiAsync VnUartPacket_determineAsciiAsyncType(VnUartPacket* packet); + +/** \defgroup uartPacketAsciiAsyncParsers UART ASCII Asynchronous Packet Parsers + * \brief This group of functions allow parsing of ASCII asynchronous data + * packets from VectorNav sensors. + * + * The units are not specified for the out parameters since these functions do + * a simple conversion operation from the received packet string. Please + * consult the appropriate sensor user manual for details about the units + * returned by the sensor. + * + * \{ + */ + +/** \brief Parses a VNYPR asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + */ +void VnUartPacket_parseVNYPR(VnUartPacket* packet, vec3f *yawPitchRoll); + +/** \brief Parses a VNQTN asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] quaternion The quaternion values in the packet. + */ +void VnUartPacket_parseVNQTN(VnUartPacket* packet, vec4f *quaternion); + +#ifdef EXTRA + +/** \brief Parses a VNQTM asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] quaternion The quaternion values in the packet. + * \param[out] magnetic The magnetic values in the packet. + */ +void VnUartPacket_parseVNQTM(VnUartPacket* packet, vec4f *quaternion, vec3f *magnetic); + +/** \brief Parses a VNQTA asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] quaternion The quaternion values in the packet. + * \param[out] acceleration The acceleration values in the packet. + */ +void VnUartPacket_parseVNQTA(VnUartPacket* packet, vec4f *quaternion, vec3f *acceleration); + +/** \brief Parses a VNQTR asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] quaternion The quaternion values in the packet. + * \param[out] angularRate The angular rate values in the packet. + */ +void VnUartPacket_parseVNQTR(VnUartPacket* packet, vec4f *quaternion, vec3f *angularRate); + +/** \brief Parses a VNQMA asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] quaternion The quaternion values in the packet. + * \param[out] magnetic The magnetic values in the packet. + * \param[out] acceleration The acceleration values in the packet. + */ +void VnUartPacket_parseVNQMA(VnUartPacket* packet, vec4f *quaternion, vec3f *magnetic, vec3f *acceleration); + +/** \brief Parses a VNQAR asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] quaternion The quaternion values in the packet. + * \param[out] acceleration The acceleration values in the packet. + * \param[out] angularRate The angular rate values in the packet. + */ +void VnUartPacket_parseVNQAR(VnUartPacket* packet, vec4f *quaternion, vec3f *acceleration, vec3f *angularRate); + +#endif + +/** \brief Parses a VNQMR asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] quaternion The quaternion values in the packet. + * \param[out] magnetic The magnetic values in the packet. + * \param[out] acceleration The acceleration values in the packet. + * \param[out] angularRate The angular rate values in the packet. + */ +void VnUartPacket_parseVNQMR(VnUartPacket* packet, vec4f *quaternion, vec3f *magnetic, vec3f *acceleration, vec3f *angularRate); + +#ifdef EXTRA + +/** \brief Parses a VNDCM asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] dcm The directional cosine matrix values in the packet. + */ +void VnUartPacket_parseVNDCM(VnUartPacket* packet, mat3f *dcm); + +#endif + +/** \brief Parses a VNMAG asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] magnetic The magnetic values in the packet. + */ +void VnUartPacket_parseVNMAG(VnUartPacket* packet, vec3f *magnetic); + +/** \brief Parses a VNACC asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] acceleration The acceleration values in the packet. + */ +void VnUartPacket_parseVNACC(VnUartPacket* packet, vec3f *acceleration); + +/** \brief Parses a VNGYR asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] angularRate The angular rate values in the packet. + */ +void VnUartPacket_parseVNGYR(VnUartPacket* packet, vec3f *angularRate); + +/** \brief Parses a VNMAR asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] magnetic The magnetic values in the packet. + * \param[out] acceleration The acceleration values in the packet. + * \param[out] angularRate The angular rate values in the packet. + */ +void VnUartPacket_parseVNMAR(VnUartPacket* packet, vec3f *magnetic, vec3f *acceleration, vec3f *angularRate); + +/** \brief Parses a VNYMR asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + * \param[out] magnetic The magnetic values in the packet. + * \param[out] acceleration The acceleration values in the packet. + * \param[out] angularRate The angular rate values in the packet. */ +VnError VnUartPacket_parseVNYMR( + VnUartPacket* packet, + vec3f *yawPitchRoll, + vec3f *magnetic, + vec3f *acceleration, + vec3f *angularRate); + +/** \brief Parses a VNYMR asynchronous packet. +* +* \param[in] packet The packet to extract the values from. +* \param[in] packetLength The number of bytes in the packet buffer. +* \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. +* \param[out] magnetic The magnetic values in the packet. +* \param[out] acceleration The acceleration values in the packet. +* \param[out] angularRate The angular rate values in the packet. */ +/*VnError VnUartPacket_parseVNYMR( + uint8_t* packetBuf, + size_t packetLen, + vec3f *yawPitchRoll, + vec3f *magnetic, + vec3f *acceleration, + vec3f *angularRate);*/ + + +#ifdef EXTRA + +/** \brief Parses a VNYCM asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + * \param[out] magnetic The magnetic values in the packet. + * \param[out] acceleration The acceleration values in the packet. + * \param[out] angularRate The angular rate values in the packet. + * \param[out] temperature The temperature value in the packet. + */ +void VnUartPacket_parseVNYCM(VnUartPacket* packet, vec3f *yawPitchRoll, vec3f *magnetic, vec3f *acceleration, vec3f *angularRate, float *temperature); + +#endif + +/** \brief Parses a VNYBA asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + * \param[out] accelerationBody The acceleration body values in the packet. + * \param[out] angularRate The angular rate values in the packet. + */ +void VnUartPacket_parseVNYBA(VnUartPacket* packet, vec3f *yawPitchRoll, vec3f *accelerationBody, vec3f *angularRate); + +/** \brief Parses a VNYIA asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + * \param[out] accelerationInertial The acceleration inertial values in the packet. + * \param[out] angularRate The angular rate values in the packet. + */ +void VnUartPacket_parseVNYIA(VnUartPacket* packet, vec3f *yawPitchRoll, vec3f *accelerationInertial, vec3f *angularRate); + +#ifdef EXTRA + +/** \brief Parses a VNICM asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + * \param[out] magnetic The magnetic values in the packet. + * \param[out] accelerationInertial The acceleration inertial values in the packet. + * \param[out] angularRate The angular rate values in the packet. + */ +void VnUartPacket_parseVNICM(VnUartPacket* packet, vec3f *yawPitchRoll, vec3f *magnetic, vec3f *accelerationInertial, vec3f *angularRate); + +#endif + +/** \brief Parses a VNIMU asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] magneticUncompensated The uncompensated magnetic values in the packet. + * \param[out] accelerationUncompensated The uncompensated acceleration values in the packet. + * \param[out] angularRateUncompensated The uncompensated angular rate values in the packet. + * \param[out] temperature The temperature value in the packet. + * \param[out] pressure The pressure value in the packet. + */ +void VnUartPacket_parseVNIMU(VnUartPacket* packet, vec3f *magneticUncompensated, vec3f *accelerationUncompensated, vec3f *angularRateUncompensated, float *temperature, float *pressure); + +/** \brief Parses a VNGPS asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] time The time value in the packet. + * \param[out] week The week value in the packet. + * \param[out] gpsFix The GPS fix value in the packet. + * \param[out] numSats The NumSats value in the packet. + * \param[out] lla The latitude, longitude and altitude values in the packet. + * \param[out] nedVel The NED velocity values in the packet. + * \param[out] nedAcc The NED position accuracy values in the packet. + * \param[out] speedAcc The SpeedAcc value in the packet. + * \param[out] timeAcc The TimeAcc value in the packet. + */ +void VnUartPacket_parseVNGPS(VnUartPacket* packet, double *time, uint16_t *week, uint8_t *gpsFix, uint8_t *numSats, vec3d *lla, vec3f *nedVel, vec3f *nedAcc, float *speedAcc, float *timeAcc); + +/** \brief Parses a VNINS asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] time The time value in the packet. + * \param[out] week The week value in the packet. + * \param[out] status The status value in the packet. + * \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. + * \param[out] lla The latitude, longitude, altitude values in the packet. + * \param[out] nedVel The NED velocity values in the packet. + * \param[out] attUncertainty The attitude uncertainty value in the packet. + * \param[out] posUncertainty The position uncertainty value in the packet. + * \param[out] velUncertainty The velocity uncertainty value in the packet. + */ +void VnUartPacket_parseVNINS(VnUartPacket* packet, double *time, uint16_t *week, uint16_t *status, vec3f *yawPitchRoll, vec3d *lla, vec3f *nedVel, float *attUncertainty, float *posUncertainty, float *velUncertainty); + +/** \brief Parses a VNINE asynchronous packet. +* +* \param[in] packet The packet to extract the values from. +* \param[out] time The time value in the packet. +* \param[out] week The week value in the packet. +* \param[out] status The status value in the packet. +* \param[out] yawPitchRoll The yaw, pitch, roll values in the packet. +* \param[out] position The latitude, longitude, altitude values in the packet. +* \param[out] velocity The NED velocity values in the packet. +* \param[out] attUncertainty The attitude uncertainty value in the packet. +* \param[out] posUncertainty The position uncertainty value in the packet. +* \param[out] velUncertainty The velocity uncertainty value in the packet. */ +void VnUartPacket_parseVNINE(VnUartPacket* packet, double *time, uint16_t *week, uint16_t *status, vec3f *yawPitchRoll, vec3d *position, vec3f *velocity, float *attUncertainty, float *posUncertainty, float *velUncertainty); + +/** \brief Parse a VNISL asynchronous packet. +* +* \param[in] packet The packet to extract the values from. +* \param[out] ypr The yaw, pitch, roll values in the packet. +* \param[out] lla The latitude, longitude, altitude values in the packet. +* \param[out] velocity The velocity values in the packet. +* \param[out] acceleration The acceleration values in the packet. +* \param[out] angularRate The angular rate values in the packet. */ +void VnUartPacket_parseVNISL(VnUartPacket* packet, vec3f* ypr, vec3d* lla, vec3f* velocity, vec3f* acceleration, vec3f* angularRate); + +/** \brief Parse a VNISE asynchronous packet. +* +* \param[in] packet The packet to extract the values from. +* \param[out] ypr The yaw, pitch, roll values in the packet. +* \param[out] position The ECEF position values in the packet. +* \param[out] velocity The ECEF velocity values in the packet. +* \param[out] acceleration The acceleration values in the packet. +* \param[out] angularRate The angular rate values in the packet. */ +void VnUartPacket_parseVNISE(VnUartPacket* packet, vec3f* ypr, vec3d* position, vec3f* velocity, vec3f* acceleration, vec3f* angularRate); + +#ifdef EXTRA + +/** \brief Parses a VNRAW asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] magneticVoltage The magnetic voltage values in the packet. + * \param[out] accelerationVoltage The acceleration voltage values in the packet. + * \param[out] angularRateVoltage The angular rate voltage values in the packet. + * \param[out] temperatureVoltage The temperature voltage value in the packet. + */ +void VnUartPacket_parseVNRAW(VnUartPacket* packet, vec3f *magneticVoltage, vec3f *accelerationVoltage, vec3f *angularRateVoltage, float *temperatureVoltage); + +/** \brief Parses a VNCMV asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] magneticUncompensated The uncompensated magnetic values in the packet. + * \param[out] accelerationUncompensated The uncompensated acceleration values in the packet. + * \param[out] angularRateUncompensated The uncompensated angular rate values in the packet. + * \param[out] temperature The temperature value in the packet. + */ +void VnUartPacket_parseVNCMV(VnUartPacket* packet, vec3f *magneticUncompensated, vec3f *accelerationUncompensated, vec3f *angularRateUncompensated, float *temperature); + +/** \brief Parses a VNSTV asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] quaternion The quaternion values in the packet. + * \param[out] angularRateBias The angular rate bias values in the packet. + */ +void VnUartPacket_parseVNSTV(VnUartPacket* packet, vec4f *quaternion, vec3f *angularRateBias); + +/** \brief Parses a VNCOV asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] attitudeVariance The attitude variance values in the packet. + * \param[out] angularRateBiasVariance The angular rate bias variance values in the packet. + */ +void VnUartPacket_parseVNCOV(VnUartPacket* packet, vec3f *attitudeVariance, vec3f *angularRateBiasVariance); + +#endif + +/** \brief Parses a VNGPE asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] tow The tow value in the packet. + * \param[out] week The week value in the packet. + * \param[out] gpsFix The GPS fix value in the packet. + * \param[out] numSats The numSats value in the packet. + * \param[out] position The ECEF position values in the packet. + * \param[out] velocity The ECEF velocity values in the packet. + * \param[out] posAcc The PosAcc values in the packet. + * \param[out] speedAcc The SpeedAcc value in the packet. + * \param[out] timeAcc The TimeAcc value in the packet. + */ +void VnUartPacket_parseVNGPE(VnUartPacket* packet, double *tow, uint16_t *week, uint8_t *gpsFix, uint8_t *numSats, vec3d *position, vec3f *velocity, vec3f *posAcc, float *speedAcc, float *timeAcc); + +/** \brief Parses a VNDTV asynchronous packet. + * + * \param[in] packet The packet to extract the values from. + * \param[out] deltaTime The DeltaTime value in the packet. + * \param[out] deltaTheta The DeltaTheta values in the packet. + * \param[out] deltaVelocity The DeltaVelocity values in the packet. + */ +void VnUartPacket_parseVNDTV(VnUartPacket* packet, float *deltaTime, vec3f *deltaTheta, vec3f *deltaVelocity); + +/** \} */ + +/** \brief Generic function for making register read commands. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating + * the command. + * \param[in] registerId The VectorNav sensor's register ID to read. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. + */ +VnError vnuart_genread( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + uint16_t registerId, + size_t *cmdSize); + +/** \defgroup uart_genread_functions UART Generate Read Functions + * \brief This set of functions will generate command strings for reading + * registers on VectorNav sensors. + * + * These functions take the form of + * VnError VnUartPacket_genReadXXX(char *buffer, size_t bufferSize, VnErrorDetection errorDetection, size_t *cmdSize) + * where XXX is replaced with the name of the register, buffer is provided by + * the user to fill with the generated command, bufferSize is the number of + * bytes in the provided buffer, errorDetection indicates the type of + * error-detection to generate the command with, and cmdSize is returned back + * to the caller to indicate the total size of the generated command placed in + * buffer. + * + * \{ +*/ + +/** \brief Generates a command to read the Binary Output 1 register on a VectorNav sensor. +* +* \param[in] buffer Caller provided buffer to place the generated command. +* \param[in] bufferSize Number of bytes available in the provided buffer. +* \param[in] errorDetection The type of error-detection to use in generating the command. +* \param[out] cmdSize The total number bytes in the generated command. +* \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadBinaryOutput1( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Binary Output 2 register on a VectorNav sensor. +* +* \param[in] buffer Caller provided buffer to place the generated command. +* \param[in] bufferSize Number of bytes available in the provided buffer. +* \param[in] errorDetection The type of error-detection to use in generating the command. +* \param[out] cmdSize The total number bytes in the generated command. +* \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadBinaryOutput2( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Binary Output 3 register on a VectorNav sensor. +* +* \param[in] buffer Caller provided buffer to place the generated command. +* \param[in] bufferSize Number of bytes available in the provided buffer. +* \param[in] errorDetection The type of error-detection to use in generating the command. +* \param[out] cmdSize The total number bytes in the generated command. +* \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadBinaryOutput3( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +#ifdef EXTRA + +/** \brief Generates a command to read the Binary Output 4 register on a VectorNav sensor. +* +* \param[in] buffer Caller provided buffer to place the generated command. +* \param[in] bufferSize Number of bytes available in the provided buffer. +* \param[in] errorDetection The type of error-detection to use in generating the command. +* \param[out] cmdSize The total number bytes in the generated command. +* \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadBinaryOutput4( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Binary Output 5 register on a VectorNav sensor. +* +* \param[in] buffer Caller provided buffer to place the generated command. +* \param[in] bufferSize Number of bytes available in the provided buffer. +* \param[in] errorDetection The type of error-detection to use in generating the command. +* \param[out] cmdSize The total number bytes in the generated command. +* \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadBinaryOutput5( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +#endif + +/** \brief Generates a command to write sensor settings to non-volitile memory. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genCmdWriteSettings( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to restore the sensor to factory settings. +* +* \param[in] buffer Caller provided buffer to place the generated command. +* \param[in] bufferSize Number of bytes available in the provided buffer. +* \param[in] errorDetection The type of error-detection to use in generating the command. +* \param[out] cmdSize The total number bytes in the generated command. +* \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genCmdRestoreFactorySettings( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to reset the sensor. +* +* \param[in] buffer Caller provided buffer to place the generated command. +* \param[in] bufferSize Number of bytes available in the provided buffer. +* \param[in] errorDetection The type of error-detection to use in generating the command. +* \param[out] cmdSize The total number bytes in the generated command. +* \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genCmdReset( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to put the sensor in Firmware Update mode. +* +* \param[in] buffer Caller provided buffer to place the generated command. +* \param[in] bufferSize Number of bytes available in the provided buffer. +* \param[in] errorDetection The type of error-detection to use in generating the command. +* \param[out] cmdSize The total number bytes in the generated command. +* \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genCmdFirmwareUpdate( + uint8_t* buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t* cmdSize); + +/** \brief Generates a command to tare the sensor. +* +* \param[in] buffer Caller provided buffer to place the generated command. +* \param[in] bufferSize Number of bytes available in the provided buffer. +* \param[in] errorDetection The type of error-detection to use in generating the command. +* \param[out] cmdSize The total number bytes in the generated command. +* \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genCmdTare( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to set the gyro bias. +* +* \param[in] buffer Caller provided buffer to place the generated command. +* \param[in] bufferSize Number of bytes available in the provided buffer. +* \param[in] errorDetection The type of error-detection to use in generating the command. +* \param[out] cmdSize The total number bytes in the generated command. +* \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genCmdSetGyroBias( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to alert the sensor of a known magnetic disturbance. +* +* \param[in] buffer Caller provided buffer to place the generated command. +* \param[in] bufferSize Number of bytes available in the provided buffer. +* \param[in] errorDetection The type of error-detection to use in generating the command. +* \param[in] disturbancePresent Indicate the presence of a magnetic disturbance. +* \param[out] cmdSize The total number bytes in the generated command. +* \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genCmdKnownMagneticDisturbance( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + bool disturbancePresent, + size_t *cmdSize); + +/** \brief Generates a command to alert the sensor of a known acceleration disturbance. +* +* \param[in] buffer Caller provided buffer to place the generated command. +* \param[in] bufferSize Number of bytes available in the provided buffer. +* \param[in] errorDetection The type of error-detection to use in generating the command. +* \param[in] disturbancePresent Indicate the presence of an acceleration disturbance. +* \param[out] cmdSize The total number bytes in the generated command. +* \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genCmdKnownAccelerationDisturbance( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + bool disturbancePresent, + size_t *cmdSize); + +/** \brief Generates a command to read the User Tag register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadUserTag( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Model Number register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadModelNumber( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Hardware Revision register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadHardwareRevision( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Serial Number register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadSerialNumber( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Firmware Version register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadFirmwareVersion( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Serial Baud Rate register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadSerialBaudRate( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Async Data Output Type register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadAsyncDataOutputType( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Async Data Output Frequency register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadAsyncDataOutputFrequency( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Yaw Pitch Roll register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadYawPitchRoll( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Attitude Quaternion register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadAttitudeQuaternion( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Quaternion, Magnetic, Acceleration and Angular Rates register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadQuaternionMagneticAccelerationAndAngularRates( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Magnetic Measurements register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadMagneticMeasurements( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Acceleration Measurements register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadAccelerationMeasurements( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Angular Rate Measurements register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadAngularRateMeasurements( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Magnetic, Acceleration and Angular Rates register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadMagneticAccelerationAndAngularRates( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Magnetic and Gravity Reference Vectors register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadMagneticAndGravityReferenceVectors( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Magnetometer Compensation register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadMagnetometerCompensation( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Acceleration Compensation register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadAccelerationCompensation( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Reference Frame Rotation register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadReferenceFrameRotation( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadYawPitchRollMagneticAccelerationAndAngularRates( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Communication Protocol Control register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadCommunicationProtocolControl( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Synchronization Control register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadSynchronizationControl( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Synchronization Status register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadSynchronizationStatus( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the VPE Basic Control register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadVpeBasicControl( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the VPE Magnetometer Basic Tuning register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadVpeMagnetometerBasicTuning( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the VPE Accelerometer Basic Tuning register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadVpeAccelerometerBasicTuning( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Magnetometer Calibration Control register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadMagnetometerCalibrationControl( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Calculated Magnetometer Calibration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadCalculatedMagnetometerCalibration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Velocity Compensation Measurement register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadVelocityCompensationMeasurement( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Velocity Compensation Control register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadVelocityCompensationControl( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the IMU Measurements register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadImuMeasurements( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the GPS Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadGpsConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the GPS Antenna Offset register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadGpsAntennaOffset( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the GPS Solution - LLA register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadGpsSolutionLla( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the GPS Solution - ECEF register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadGpsSolutionEcef( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the INS Solution - LLA register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadInsSolutionLla( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the INS Solution - ECEF register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadInsSolutionEcef( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the INS Basic Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadInsBasicConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the INS Basic Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadInsBasicConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the INS State - LLA register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadInsStateLla( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the INS State - ECEF register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadInsStateEcef( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Startup Filter Bias Estimate register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadStartupFilterBiasEstimate( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Delta Theta and Delta Velocity register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadDeltaThetaAndDeltaVelocity( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Delta Theta and Delta Velocity Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadDeltaThetaAndDeltaVelocityConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Reference Vector Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadReferenceVectorConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Gyro Compensation register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadGyroCompensation( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the IMU Filtering Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadImuFilteringConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the GPS Compass Baseline register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadGpsCompassBaseline( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the GPS Compass Estimated Baseline register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadGpsCompassEstimatedBaseline( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadYawPitchRollTrueBodyAccelerationAndAngularRates( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \brief Generates a command to read the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genReadYawPitchRollTrueInertialAccelerationAndAngularRates( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize); + +/** \} */ + +/** \brief Generic function for making write register commands. + * + * The format parameters uses formats that look like "U4F4F8X1" which + * represents an unsigned 32-bit integer, followed by a 4-byte float + * (single-precision), an 8-byte float (double-precision, and finally a 1-byte + * flag field. The table below shows the possible combinations of formats. + * + * + * + * + * + * + * + * + *
FormatDescription
U11-byte unsigned integer (uint8_t)
U22-byte unsigned integer (uint16_t)
U44-byte unsigned integer (uint32_t)
F44-byte floating-point (single-precision)
F88-byte floating-point (double-precision)
+ * + * An example of using the function for setting the VectorNav sensor's baudrate + * to 9600 is shown below. + * + * \code + * char buffer[256]; + * size_t cmdSize; + * + * VnUartPacket_genWrite( + * buffer, + * 256, + * VNERR_DETECTION_CHECKSUM_8BIT_XOR, + * 5, + * &cmdSize, + * "U4", + * 9600); + * + * buffer should contain "$VNWRG,5,9600*60\r\n" now. + * \endcode + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetection The type of error-detection to use in generating + * the command. + * \param[in] registerId The VectorNav sensor's register ID to read. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] format Format specifier for the variable arguments. + * \param[in] ... Variable argument list for generating the command. + * \return Indicates any errors encountered. + */ +VnError VnUartPacket_genWrite( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + uint16_t registerId, + size_t *cmdSize, + char const *format, + ...); + +/** \defgroup uart_genwrite_functions UART Generate Write Functions + * \brief This set of functions will generate command strings for writing to + * VectorNav sensor registers. + * + * These functions take the form shown below. XXX is replaced by the name + * of the register, buffer is provided by the user to be filled with the + * generated command, bufferSize is the number of bytes in the provided + * buffer, errorDetection indicates the type of error-detection to + * generate the command with, cmdSize is returned to the user to indicate + * the number of bytes of the generated command, and [Variable argument list] + * varies with the specified register being written to. + * + * \code + * VnError VnUartPacket_genWriteXXX( + * char *buffer, + * size_t bufferSize, + * VnErrorDetection errorDetection, + * size_t *cmdSize, + * [Variable argument list]); + * \endcode + * + * \{ + */ + +/** \brief Generates a command to write to the Binary Output 1 register. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] asyncMode The register's async mode. + * \param[in] rateDivisor The register's rate divisor field. + * \param[in] commonField The flags for Group 1 (Common) field. + * \param[in] timeField The flags for Group 2 (Time) field. + * \param[in] imuField The flags for Group 3 (IMU) field. + * \param[in] gpsField The flags for Group 4 (GPS) field. + * \param[in] attitudeField The flags for Group 5 (Attitude) field. + * \param[in] insField The flags for Group 6 (INS) field. + * \return Indicates any errors encountered. + */ +VnError VnUartPacket_genWriteBinaryOutput1( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t commonField, + uint16_t timeField, + uint16_t imuField, + uint16_t gpsField, + uint16_t attitudeField, + uint16_t insField, + uint16_t gps2Field); + +/** \brief Generates a command to write to the Binary Output 2 register. +* +* \param[in] buffer Caller provided buffer to place the generated command. +* \param[in] bufferSize Number of bytes available in the provided buffer. +* \param[in] errorDetectionMode The type of error-detection to use in generating the command. +* \param[out] cmdSize The total number bytes in the generated command. +* \param[in] asyncMode The register's async mode. +* \param[in] rateDivisor The register's rate divisor field. +* \param[in] commonField The flags for Group 1 (Common) field. +* \param[in] timeField The flags for Group 2 (Time) field. +* \param[in] imuField The flags for Group 3 (IMU) field. +* \param[in] gpsField The flags for Group 4 (GPS) field. +* \param[in] attitudeField The flags for Group 5 (Attitude) field. +* \param[in] insField The flags for Group 6 (INS) field. +* \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genWriteBinaryOutput2( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t commonField, + uint16_t timeField, + uint16_t imuField, + uint16_t gpsField, + uint16_t attitudeField, + uint16_t insField, + uint16_t gps2Field); + +/** \brief Generates a command to write to the Binary Output 3 register. +* +* \param[in] buffer Caller provided buffer to place the generated command. +* \param[in] bufferSize Number of bytes available in the provided buffer. +* \param[in] errorDetectionMode The type of error-detection to use in generating the command. +* \param[out] cmdSize The total number bytes in the generated command. +* \param[in] asyncMode The register's async mode. +* \param[in] rateDivisor The register's rate divisor field. +* \param[in] commonField The flags for Group 1 (Common) field. +* \param[in] timeField The flags for Group 2 (Time) field. +* \param[in] imuField The flags for Group 3 (IMU) field. +* \param[in] gpsField The flags for Group 4 (GPS) field. +* \param[in] attitudeField The flags for Group 5 (Attitude) field. +* \param[in] insField The flags for Group 6 (INS) field. +* \return Indicates any errors encountered. +*/ +VnError VnUartPacket_genWriteBinaryOutput3( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t commonField, + uint16_t timeField, + uint16_t imuField, + uint16_t gpsField, + uint16_t attitudeField, + uint16_t insField, + uint16_t gps2Field); + +#ifdef EXTRA + +/** \brief Generates a command to write to the Binary Output 4 register. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] asyncMode The register's async mode. + * \param[in] rateDivisor The register's rate divisor field. + * \param[in] commonField The flags for Group 1 (Common) field. + * \param[in] timeField The flags for Group 2 (Time) field. + * \param[in] imuField The flags for Group 3 (IMU) field. + * \param[in] gpsField The flags for Group 4 (GPS) field. + * \param[in] attitudeField The flags for Group 5 (Attitude) field. + * \param[in] insField The flags for Group 6 (INS) field. + * \return Indicates any errors encountered. + */ +VnError VnUartPacket_genWriteBinaryOutput4( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t commonField, + uint16_t timeField, + uint16_t imuField, + uint16_t gpsField, + uint16_t attitudeField, + uint16_t insField, + uint16_t gps2Field); + +/** \brief Generates a command to write to the Binary Output 5 register. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] asyncMode The register's async mode. + * \param[in] rateDivisor The register's rate divisor field. + * \param[in] commonField The flags for Group 1 (Common) field. + * \param[in] timeField The flags for Group 2 (Time) field. + * \param[in] imuField The flags for Group 3 (IMU) field. + * \param[in] gpsField The flags for Group 4 (GPS) field. + * \param[in] attitudeField The flags for Group 5 (Attitude) field. + * \param[in] insField The flags for Group 6 (INS) field. + * \return Indicates any errors encountered. + */ +VnError VnUartPacket_genWriteBinaryOutput5( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t commonField, + uint16_t timeField, + uint16_t imuField, + uint16_t gpsField, + uint16_t attitudeField, + uint16_t insField, + uint16_t gps2Field); + +#endif + +/** \brief Generates a command to write to firmware update record to the VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] record Firmware Update Record to write to the sensor. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteFirmwareUpdate( + char* buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t* cmdSize, + char* record); + +/** \brief Generates a command to write to the User Tag register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] tag The register's Tag field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteUserTag( + char* buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t* cmdSize, + char* tag); + +/** \brief Generates a command to write to the Serial Baud Rate register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] baudrate The register's Baud Rate field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteSerialBaudRate( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint32_t baudrate); + +/** \brief Generates a command to write to the Serial Baud Rate register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] baudrate The register's Baud Rate field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteSerialBaudRateWithOptions( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint32_t baudrate); + +/** \brief Generates a command to write to the Async Data Output Type register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] ador The register's ADOR field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteAsyncDataOutputType( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint32_t ador); + +/** \brief Generates a command to write to the Async Data Output Type register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] ador The register's ADOR field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteAsyncDataOutputTypeWithOptions( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint32_t ador); + +/** \brief Generates a command to write to the Async Data Output Frequency register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] adof The register's ADOF field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteAsyncDataOutputFrequency( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint32_t adof); + +/** \brief Generates a command to write to the Async Data Output Frequency register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] adof The register's ADOF field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteAsyncDataOutputFrequencyWithOptions( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint32_t adof); + +/** \brief Generates a command to write to the Magnetic and Gravity Reference Vectors register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] magRef The register's MagRef field. + * \param[in] accRef The register's AccRef field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteMagneticAndGravityReferenceVectors( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + vec3f magRef, + vec3f accRef); + +/** \brief Generates a command to write to the Magnetometer Compensation register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] c The register's C field. + * \param[in] b The register's B field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteMagnetometerCompensation( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + mat3f c, + vec3f b); + +/** \brief Generates a command to write to the Acceleration Compensation register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] c The register's C field. + * \param[in] b The register's B field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteAccelerationCompensation( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + mat3f c, + vec3f b); + +/** \brief Generates a command to write to the Reference Frame Rotation register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] c The register's C field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteReferenceFrameRotation( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + mat3f c); + +/** \brief Generates a command to write to the Communication Protocol Control register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] serialCount The register's SerialCount field. + * \param[in] serialStatus The register's SerialStatus field. + * \param[in] spiCount The register's SPICount field. + * \param[in] spiStatus The register's SPIStatus field. + * \param[in] serialChecksum The register's SerialChecksum field. + * \param[in] spiChecksum The register's SPIChecksum field. + * \param[in] errorMode The register's ErrorMode field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteCommunicationProtocolControl( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint8_t serialCount, + uint8_t serialStatus, + uint8_t spiCount, + uint8_t spiStatus, + uint8_t serialChecksum, + uint8_t spiChecksum, + uint8_t errorMode); + +/** \brief Generates a command to write to the Synchronization Control register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] syncInMode The register's SyncInMode field. + * \param[in] syncInEdge The register's SyncInEdge field. + * \param[in] syncInSkipFactor The register's SyncInSkipFactor field. + * \param[in] reserved1 The register's RESERVED1 field. + * \param[in] syncOutMode The register's SyncOutMode field. + * \param[in] syncOutPolarity The register's SyncOutPolarity field. + * \param[in] syncOutSkipFactor The register's SyncOutSkipFactor field. + * \param[in] syncOutPulseWidth The register's SyncOutPulseWidth field. + * \param[in] reserved2 The register's RESERVED2 field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteSynchronizationControl( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint8_t syncInMode, + uint8_t syncInEdge, + uint16_t syncInSkipFactor, + uint32_t reserved1, + uint8_t syncOutMode, + uint8_t syncOutPolarity, + uint16_t syncOutSkipFactor, + uint32_t syncOutPulseWidth, + uint32_t reserved2); + +/** \brief Generates a command to write to the Synchronization Status register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] syncInCount The register's SyncInCount field. + * \param[in] syncInTime The register's SyncInTime field. + * \param[in] syncOutCount The register's SyncOutCount field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteSynchronizationStatus( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint32_t syncInCount, + uint32_t syncInTime, + uint32_t syncOutCount); + +/** \brief Generates a command to write to the VPE Basic Control register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] enable The register's Enable field. + * \param[in] headingMode The register's HeadingMode field. + * \param[in] filteringMode The register's FilteringMode field. + * \param[in] tuningMode The register's TuningMode field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteVpeBasicControl( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint8_t enable, + uint8_t headingMode, + uint8_t filteringMode, + uint8_t tuningMode); + +/** \brief Generates a command to write to the VPE Magnetometer Basic Tuning register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] baseTuning The register's BaseTuning field. + * \param[in] adaptiveTuning The register's AdaptiveTuning field. + * \param[in] adaptiveFiltering The register's AdaptiveFiltering field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteVpeMagnetometerBasicTuning( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + vec3f baseTuning, + vec3f adaptiveTuning, + vec3f adaptiveFiltering); + +/** \brief Generates a command to write to the VPE Accelerometer Basic Tuning register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] baseTuning The register's BaseTuning field. + * \param[in] adaptiveTuning The register's AdaptiveTuning field. + * \param[in] adaptiveFiltering The register's AdaptiveFiltering field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteVpeAccelerometerBasicTuning( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + vec3f baseTuning, + vec3f adaptiveTuning, + vec3f adaptiveFiltering); + +/** \brief Generates a command to write to the Magnetometer Calibration Control register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] hsiMode The register's HSIMode field. + * \param[in] hsiOutput The register's HSIOutput field. + * \param[in] convergeRate The register's ConvergeRate field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteMagnetometerCalibrationControl( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint8_t hsiMode, + uint8_t hsiOutput, + uint8_t convergeRate); + +/** \brief Generates a command to write to the Velocity Compensation Measurement register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] velocity The register's Velocity field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteVelocityCompensationMeasurement( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + vec3f velocity); + +/** \brief Generates a command to write to the Velocity Compensation Control register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] mode The register's Mode field. + * \param[in] velocityTuning The register's VelocityTuning field. + * \param[in] rateTuning The register's RateTuning field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteVelocityCompensationControl( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint8_t mode, + float velocityTuning, + float rateTuning); + +/** \brief Generates a command to write to the GPS Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] mode The register's Mode field. + * \param[in] ppsSource The register's PpsSource field. + * \param[in] reserved1 The register's Reserved1 field. + * \param[in] reserved2 The register's Reserved2 field. + * \param[in] reserved3 The register's Reserved3 field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteGpsConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint8_t mode, + uint8_t ppsSource, + uint8_t reserved1, + uint8_t reserved2, + uint8_t reserved3); + +/** \brief Generates a command to write to the GPS Antenna Offset register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] position The register's Position field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteGpsAntennaOffset( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + vec3f position); + +/** \brief Generates a command to write to the INS Basic Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] scenario The register's Scenario field. + * \param[in] ahrsAiding The register's AhrsAiding field. + * \param[in] estBaseline The register's EstBaseline field. + * \param[in] resv2 The register's Resv2 field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteInsBasicConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint8_t scenario, + uint8_t ahrsAiding, + uint8_t estBaseline, + uint8_t resv2); + +/** \brief Generates a command to write to the Startup Filter Bias Estimate register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] gyroBias The register's GyroBias field. + * \param[in] accelBias The register's AccelBias field. + * \param[in] pressureBias The register's PressureBias field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteStartupFilterBiasEstimate( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + vec3f gyroBias, + vec3f accelBias, + float pressureBias); + +/** \brief Generates a command to write to the Delta Theta and Delta Velocity Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] integrationFrame The register's IntegrationFrame field. + * \param[in] gyroCompensation The register's GyroCompensation field. + * \param[in] accelCompensation The register's AccelCompensation field. + * \param[in] reserved1 The register's Reserved1 field. + * \param[in] reserved2 The register's Reserved2 field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteDeltaThetaAndDeltaVelocityConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint8_t integrationFrame, + uint8_t gyroCompensation, + uint8_t accelCompensation, + uint8_t reserved1, + uint16_t reserved2); + +/** \brief Generates a command to write to the Reference Vector Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] useMagModel The register's UseMagModel field. + * \param[in] useGravityModel The register's UseGravityModel field. + * \param[in] resv1 The register's Resv1 field. + * \param[in] resv2 The register's Resv2 field. + * \param[in] recalcThreshold The register's RecalcThreshold field. + * \param[in] year The register's Year field. + * \param[in] position The register's Position field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteReferenceVectorConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint8_t useMagModel, + uint8_t useGravityModel, + uint8_t resv1, + uint8_t resv2, + uint32_t recalcThreshold, + float year, + vec3d position); + +/** \brief Generates a command to write to the Gyro Compensation register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] c The register's C field. + * \param[in] b The register's B field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteGyroCompensation( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + mat3f c, + vec3f b); + +/** \brief Generates a command to write to the IMU Filtering Configuration register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] magWindowSize The register's MagWindowSize field. + * \param[in] accelWindowSize The register's AccelWindowSize field. + * \param[in] gyroWindowSize The register's GyroWindowSize field. + * \param[in] tempWindowSize The register's TempWindowSize field. + * \param[in] presWindowSize The register's PresWindowSize field. + * \param[in] magFilterMode The register's MagFilterMode field. + * \param[in] accelFilterMode The register's AccelFilterMode field. + * \param[in] gyroFilterMode The register's GyroFilterMode field. + * \param[in] tempFilterMode The register's TempFilterMode field. + * \param[in] presFilterMode The register's PresFilterMode field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteImuFilteringConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint16_t magWindowSize, + uint16_t accelWindowSize, + uint16_t gyroWindowSize, + uint16_t tempWindowSize, + uint16_t presWindowSize, + uint8_t magFilterMode, + uint8_t accelFilterMode, + uint8_t gyroFilterMode, + uint8_t tempFilterMode, + uint8_t presFilterMode); + +/** \brief Generates a command to write to the GPS Compass Baseline register on a VectorNav sensor. + * + * \param[in] buffer Caller provided buffer to place the generated command. + * \param[in] bufferSize Number of bytes available in the provided buffer. + * \param[in] errorDetectionMode The type of error-detection to use in generating the command. + * \param[out] cmdSize The total number bytes in the generated command. + * \param[in] position The register's Position field. + * \param[in] uncertainty The register's Uncertainty field. + * \return Indicates any errors encountered. */ +VnError VnUartPacket_genWriteGpsCompassBaseline( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + vec3f position, + vec3f uncertainty); + +/** \brief Parses an error packet to get the error type. + * + * \param[in] packet The associated packet. + * \param[out] error The reported error. + */ +void VnUartPacket_parseError(VnUartPacket *packet, uint8_t *error); + +/** \brief Parses an error packet to get the error type. +* +* \param[in] packet The associated packet. +* \param[out] error The reported error. +*/ +void VnUartPacket_parseErrorRaw(uint8_t *packet, uint8_t *error); + +/** \brief Parses a response from reading any of the Binary Output registers. + * + * \param[in] packet The associated packet. + * \param[out] asyncMode The register's AsyncMode field. + * \param[out] rateDivisor The register's RateDivisor field. + * \param[out] outputGroup The register's OutputGroup field. + * \param[out] commonField The set fields of Output Group 1 (Common) if present. + * \param[out] timeField The set fields of Output Group 2 (Time) if present. + * \param[out] imuField The set fields of Output Group 3 (IMU) if present. + * \param[out] gpsField The set fields of Output Group 4 (GPS) if present. + * \param[out] attitudeField The set fields of Output Group 5 (Attitude) if present. + * \param[out] insField The set fields of Output Group 6 (INS) if present. + * \param[out] gps2Field The set fields of Output Group 7 (GPS2) if present. + */ +void VnUartPacket_parseBinaryOutput( + VnUartPacket *packet, + uint16_t* asyncMode, + uint16_t* rateDivisor, + uint16_t* outputGroup, + uint16_t* commonField, + uint16_t* timeField, + uint16_t* imuField, + uint16_t* gpsField, + uint16_t* attitudeField, + uint16_t* insField, + uint16_t* gps2Field); + +/** \brief Parses a response from reading any of the Binary Output registers. +* +* \param[in] packet The associated packet. +* \param[out] asyncMode The register's AsyncMode field. +* \param[out] rateDivisor The register's RateDivisor field. +* \param[out] outputGroup The register's OutputGroup field. +* \param[out] commonField The set fields of Output Group 1 (Common) if present. +* \param[out] timeField The set fields of Output Group 2 (Time) if present. +* \param[out] imuField The set fields of Output Group 3 (IMU) if present. +* \param[out] gpsField The set fields of Output Group 4 (GPS) if present. +* \param[out] attitudeField The set fields of Output Group 5 (Attitude) if present. +* \param[out] insField The set fields of Output Group 6 (INS) if present. +* \param[out] gps2Field The set fields of Output Group 7 (GPS2) if present. +*/ +void VnUartPacket_parseBinaryOutputRaw( + uint8_t *packet, + uint16_t* asyncMode, + uint16_t* rateDivisor, + uint16_t* outputGroup, + uint16_t* commonField, + uint16_t* timeField, + uint16_t* imuField, + uint16_t* gpsField, + uint16_t* attitudeField, + uint16_t* insField, + uint16_t* gps2Field); + +/** \brief Parses a response from reading the User Tag register. + * + * \param[in] packet The associated packet. + * \param[out] tag The register's Tag field. + */ +void VnUartPacket_parseUserTag(VnUartPacket *packet, char* tag); + +/** \brief Parses a response from reading the User Tag register. + * + * \param[in] packet The associated packet. + * \param[out] tag The register's Tag field. + */ +void VnUartPacket_parseUserTagRaw(char *packet, char* tag); + +/** \brief Parses a response from reading the Model Number register. + * + * \param[in] packet The associated packet. + * \param[out] productName The register's Product Name field. + */ +void VnUartPacket_parseModelNumber(VnUartPacket *packet, char* productName); + +/** \brief Parses a response from reading the Model Number register. + * + * \param[in] packet The associated packet. + * \param[out] productName The register's Product Name field. + */ +void VnUartPacket_parseModelNumberRaw(char *packet, char* productName); + +/** \brief Parses a response from reading the Hardware Revision register. + * + * \param[in] packet The associated packet. + * \param[out] revision The register's Revision field. + */ +void VnUartPacket_parseHardwareRevision(VnUartPacket *packet, uint32_t* revision); + +/** \brief Parses a response from reading the Hardware Revision register. + * + * \param[in] packet The associated packet. + * \param[out] revision The register's Revision field. + */ +void VnUartPacket_parseHardwareRevisionRaw(char *packet, uint32_t* revision); + +/** \brief Parses a response from reading the Serial Number register. + * + * \param[in] packet The associated packet. + * \param[out] serialNum The register's SerialNum field. + */ +void VnUartPacket_parseSerialNumber(VnUartPacket *packet, uint32_t* serialNum); + +/** \brief Parses a response from reading the Serial Number register. + * + * \param[in] packet The associated packet. + * \param[out] serialNum The register's SerialNum field. + */ +void VnUartPacket_parseSerialNumberRaw(char *packet, uint32_t* serialNum); + +/** \brief Parses a response from reading the Firmware Version register. + * + * \param[in] packet The associated packet. + * \param[out] firmwareVersion The register's Firmware Version field. + */ +void VnUartPacket_parseFirmwareVersion(VnUartPacket *packet, char* firmwareVersion); + +/** \brief Parses a response from reading the Firmware Version register. + * + * \param[in] packet The associated packet. + * \param[out] firmwareVersion The register's Firmware Version field. + */ +void VnUartPacket_parseFirmwareVersionRaw(char *packet, char* firmwareVersion); + +/** \brief Parses a response from reading the Serial Baud Rate register. + * + * \param[in] packet The associated packet. + * \param[out] baudrate The register's Baud Rate field. + */ +void VnUartPacket_parseSerialBaudRate(VnUartPacket *packet, uint32_t* baudrate); + +/** \brief Parses a response from reading the Serial Baud Rate register. + * + * \param[in] packet The associated packet. + * \param[out] baudrate The register's Baud Rate field. + */ +void VnUartPacket_parseSerialBaudRateRaw(char *packet, uint32_t* baudrate); + +/** \brief Parses a response from reading the Async Data Output Type register. + * + * \param[in] packet The associated packet. + * \param[out] ador The register's ADOR field. + */ +void VnUartPacket_parseAsyncDataOutputType(VnUartPacket *packet, uint32_t* ador); + +/** \brief Parses a response from reading the Async Data Output Type register. + * + * \param[in] packet The associated packet. + * \param[out] ador The register's ADOR field. + */ +void VnUartPacket_parseAsyncDataOutputTypeRaw(char *packet, uint32_t* ador); + +/** \brief Parses a response from reading the Async Data Output Frequency register. + * + * \param[in] packet The associated packet. + * \param[out] adof The register's ADOF field. + */ +void VnUartPacket_parseAsyncDataOutputFrequency(VnUartPacket *packet, uint32_t* adof); + +/** \brief Parses a response from reading the Async Data Output Frequency register. + * + * \param[in] packet The associated packet. + * \param[out] adof The register's ADOF field. + */ +void VnUartPacket_parseAsyncDataOutputFrequencyRaw(char *packet, uint32_t* adof); + +/** \brief Parses a response from reading the Yaw Pitch Roll register. + * + * \param[in] packet The associated packet. + * \param[out] yawPitchRoll The register's YawPitchRoll field. + */ +void VnUartPacket_parseYawPitchRoll(VnUartPacket *packet, vec3f* yawPitchRoll); + +/** \brief Parses a response from reading the Yaw Pitch Roll register. + * + * \param[in] packet The associated packet. + * \param[out] yawPitchRoll The register's YawPitchRoll field. + */ +void VnUartPacket_parseYawPitchRollRaw(char *packet, vec3f* yawPitchRoll); + +/** \brief Parses a response from reading the Attitude Quaternion register. + * + * \param[in] packet The associated packet. + * \param[out] quat The register's Quat field. + */ +void VnUartPacket_parseAttitudeQuaternion(VnUartPacket *packet, vec4f* quat); + +/** \brief Parses a response from reading the Attitude Quaternion register. + * + * \param[in] packet The associated packet. + * \param[out] quat The register's Quat field. + */ +void VnUartPacket_parseAttitudeQuaternionRaw(char *packet, vec4f* quat); + +/** \brief Parses a response from reading the Quaternion, Magnetic, Acceleration and Angular Rates register. + * + * \param[in] packet The associated packet. + * \param[out] quat The register's Quat field. + * \param[out] mag The register's Mag field. + * \param[out] accel The register's Accel field. + * \param[out] gyro The register's Gyro field. + */ +void VnUartPacket_parseQuaternionMagneticAccelerationAndAngularRates(VnUartPacket *packet, vec4f* quat, vec3f* mag, vec3f* accel, vec3f* gyro); + +/** \brief Parses a response from reading the Quaternion, Magnetic, Acceleration and Angular Rates register. + * + * \param[in] packet The associated packet. + * \param[out] quat The register's Quat field. + * \param[out] mag The register's Mag field. + * \param[out] accel The register's Accel field. + * \param[out] gyro The register's Gyro field. + */ +void VnUartPacket_parseQuaternionMagneticAccelerationAndAngularRatesRaw(char *packet, vec4f* quat, vec3f* mag, vec3f* accel, vec3f* gyro); + +/** \brief Parses a response from reading the Magnetic Measurements register. + * + * \param[in] packet The associated packet. + * \param[out] mag The register's Mag field. + */ +void VnUartPacket_parseMagneticMeasurements(VnUartPacket *packet, vec3f* mag); + +/** \brief Parses a response from reading the Magnetic Measurements register. + * + * \param[in] packet The associated packet. + * \param[out] mag The register's Mag field. + */ +void VnUartPacket_parseMagneticMeasurementsRaw(char *packet, vec3f* mag); + +/** \brief Parses a response from reading the Acceleration Measurements register. + * + * \param[in] packet The associated packet. + * \param[out] accel The register's Accel field. + */ +void VnUartPacket_parseAccelerationMeasurements(VnUartPacket *packet, vec3f* accel); + +/** \brief Parses a response from reading the Acceleration Measurements register. + * + * \param[in] packet The associated packet. + * \param[out] accel The register's Accel field. + */ +void VnUartPacket_parseAccelerationMeasurementsRaw(char *packet, vec3f* accel); + +/** \brief Parses a response from reading the Angular Rate Measurements register. + * + * \param[in] packet The associated packet. + * \param[out] gyro The register's Gyro field. + */ +void VnUartPacket_parseAngularRateMeasurements(VnUartPacket *packet, vec3f* gyro); + +/** \brief Parses a response from reading the Angular Rate Measurements register. + * + * \param[in] packet The associated packet. + * \param[out] gyro The register's Gyro field. + */ +void VnUartPacket_parseAngularRateMeasurementsRaw(char *packet, vec3f* gyro); + +/** \brief Parses a response from reading the Magnetic, Acceleration and Angular Rates register. + * + * \param[in] packet The associated packet. + * \param[out] mag The register's Mag field. + * \param[out] accel The register's Accel field. + * \param[out] gyro The register's Gyro field. + */ +void VnUartPacket_parseMagneticAccelerationAndAngularRates(VnUartPacket *packet, vec3f* mag, vec3f* accel, vec3f* gyro); + +/** \brief Parses a response from reading the Magnetic, Acceleration and Angular Rates register. + * + * \param[in] packet The associated packet. + * \param[out] mag The register's Mag field. + * \param[out] accel The register's Accel field. + * \param[out] gyro The register's Gyro field. + */ +void VnUartPacket_parseMagneticAccelerationAndAngularRatesRaw(char *packet, vec3f* mag, vec3f* accel, vec3f* gyro); + +/** \brief Parses a response from reading the Magnetic and Gravity Reference Vectors register. + * + * \param[in] packet The associated packet. + * \param[out] magRef The register's MagRef field. + * \param[out] accRef The register's AccRef field. + */ +void VnUartPacket_parseMagneticAndGravityReferenceVectors(VnUartPacket *packet, vec3f* magRef, vec3f* accRef); + +/** \brief Parses a response from reading the Magnetic and Gravity Reference Vectors register. + * + * \param[in] packet The associated packet. + * \param[out] magRef The register's MagRef field. + * \param[out] accRef The register's AccRef field. + */ +void VnUartPacket_parseMagneticAndGravityReferenceVectorsRaw(char *packet, vec3f* magRef, vec3f* accRef); + +/** \brief Parses a response from reading the Filter Measurements Variance Parameters register. + * + * \param[in] packet The associated packet. + * \param[out] angularWalkVariance The register's Angular Walk Variance field. + * \param[out] angularRateVariance The register's Angular Rate Variance field. + * \param[out] magneticVariance The register's Magnetic Variance field. + * \param[out] accelerationVariance The register's Acceleration Variance field. + */ +void VnUartPacket_parseFilterMeasurementsVarianceParameters(VnUartPacket *packet, float* angularWalkVariance, vec3f* angularRateVariance, vec3f* magneticVariance, vec3f* accelerationVariance); + +/** \brief Parses a response from reading the Filter Measurements Variance Parameters register. + * + * \param[in] packet The associated packet. + * \param[out] angularWalkVariance The register's Angular Walk Variance field. + * \param[out] angularRateVariance The register's Angular Rate Variance field. + * \param[out] magneticVariance The register's Magnetic Variance field. + * \param[out] accelerationVariance The register's Acceleration Variance field. + */ +void VnUartPacket_parseFilterMeasurementsVarianceParametersRaw(char *packet, float* angularWalkVariance, vec3f* angularRateVariance, vec3f* magneticVariance, vec3f* accelerationVariance); + +/** \brief Parses a response from reading the Magnetometer Compensation register. + * + * \param[in] packet The associated packet. + * \param[out] c The register's C field. + * \param[out] b The register's B field. + */ +void VnUartPacket_parseMagnetometerCompensation(VnUartPacket *packet, mat3f* c, vec3f* b); + +/** \brief Parses a response from reading the Magnetometer Compensation register. + * + * \param[in] packet The associated packet. + * \param[out] c The register's C field. + * \param[out] b The register's B field. + */ +void VnUartPacket_parseMagnetometerCompensationRaw(char *packet, mat3f* c, vec3f* b); + +/** \brief Parses a response from reading the Filter Active Tuning Parameters register. + * + * \param[in] packet The associated packet. + * \param[out] magneticDisturbanceGain The register's Magnetic Disturbance Gain field. + * \param[out] accelerationDisturbanceGain The register's Acceleration Disturbance Gain field. + * \param[out] magneticDisturbanceMemory The register's Magnetic Disturbance Memory field. + * \param[out] accelerationDisturbanceMemory The register's Acceleration Disturbance Memory field. + */ +void VnUartPacket_parseFilterActiveTuningParameters(VnUartPacket *packet, float* magneticDisturbanceGain, float* accelerationDisturbanceGain, float* magneticDisturbanceMemory, float* accelerationDisturbanceMemory); + +/** \brief Parses a response from reading the Filter Active Tuning Parameters register. + * + * \param[in] packet The associated packet. + * \param[out] magneticDisturbanceGain The register's Magnetic Disturbance Gain field. + * \param[out] accelerationDisturbanceGain The register's Acceleration Disturbance Gain field. + * \param[out] magneticDisturbanceMemory The register's Magnetic Disturbance Memory field. + * \param[out] accelerationDisturbanceMemory The register's Acceleration Disturbance Memory field. + */ +void VnUartPacket_parseFilterActiveTuningParametersRaw(char *packet, float* magneticDisturbanceGain, float* accelerationDisturbanceGain, float* magneticDisturbanceMemory, float* accelerationDisturbanceMemory); + +/** \brief Parses a response from reading the Acceleration Compensation register. + * + * \param[in] packet The associated packet. + * \param[out] c The register's C field. + * \param[out] b The register's B field. + */ +void VnUartPacket_parseAccelerationCompensation(VnUartPacket *packet, mat3f* c, vec3f* b); + +/** \brief Parses a response from reading the Acceleration Compensation register. + * + * \param[in] packet The associated packet. + * \param[out] c The register's C field. + * \param[out] b The register's B field. + */ +void VnUartPacket_parseAccelerationCompensationRaw(char *packet, mat3f* c, vec3f* b); + +/** \brief Parses a response from reading the Reference Frame Rotation register. + * + * \param[in] packet The associated packet. + * \param[out] c The register's C field. + */ +void VnUartPacket_parseReferenceFrameRotation(VnUartPacket *packet, mat3f* c); + +/** \brief Parses a response from reading the Reference Frame Rotation register. + * + * \param[in] packet The associated packet. + * \param[out] c The register's C field. + */ +void VnUartPacket_parseReferenceFrameRotationRaw(char *packet, mat3f* c); + +/** \brief Parses a response from reading the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register. + * + * \param[in] packet The associated packet. + * \param[out] yawPitchRoll The register's YawPitchRoll field. + * \param[out] mag The register's Mag field. + * \param[out] accel The register's Accel field. + * \param[out] gyro The register's Gyro field. + */ +void VnUartPacket_parseYawPitchRollMagneticAccelerationAndAngularRates(VnUartPacket *packet, vec3f* yawPitchRoll, vec3f* mag, vec3f* accel, vec3f* gyro); + +/** \brief Parses a response from reading the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register. + * + * \param[in] packet The associated packet. + * \param[out] yawPitchRoll The register's YawPitchRoll field. + * \param[out] mag The register's Mag field. + * \param[out] accel The register's Accel field. + * \param[out] gyro The register's Gyro field. + */ +void VnUartPacket_parseYawPitchRollMagneticAccelerationAndAngularRatesRaw(char *packet, vec3f* yawPitchRoll, vec3f* mag, vec3f* accel, vec3f* gyro); + +/** \brief Parses a response from reading the Communication Protocol Control register. + * + * \param[in] packet The associated packet. + * \param[out] serialCount The register's SerialCount field. + * \param[out] serialStatus The register's SerialStatus field. + * \param[out] spiCount The register's SPICount field. + * \param[out] spiStatus The register's SPIStatus field. + * \param[out] serialChecksum The register's SerialChecksum field. + * \param[out] spiChecksum The register's SPIChecksum field. + * \param[out] errorMode The register's ErrorMode field. + */ +void VnUartPacket_parseCommunicationProtocolControl(VnUartPacket *packet, uint8_t* serialCount, uint8_t* serialStatus, uint8_t* spiCount, uint8_t* spiStatus, uint8_t* serialChecksum, uint8_t* spiChecksum, uint8_t* errorMode); + +/** \brief Parses a response from reading the Communication Protocol Control register. + * + * \param[in] packet The associated packet. + * \param[out] serialCount The register's SerialCount field. + * \param[out] serialStatus The register's SerialStatus field. + * \param[out] spiCount The register's SPICount field. + * \param[out] spiStatus The register's SPIStatus field. + * \param[out] serialChecksum The register's SerialChecksum field. + * \param[out] spiChecksum The register's SPIChecksum field. + * \param[out] errorMode The register's ErrorMode field. + */ +void VnUartPacket_parseCommunicationProtocolControlRaw(char *packet, uint8_t* serialCount, uint8_t* serialStatus, uint8_t* spiCount, uint8_t* spiStatus, uint8_t* serialChecksum, uint8_t* spiChecksum, uint8_t* errorMode); + +/** \brief Parses a response from reading the Synchronization Control register. + * + * \param[in] packet The associated packet. + * \param[out] syncInMode The register's SyncInMode field. + * \param[out] syncInEdge The register's SyncInEdge field. + * \param[out] syncInSkipFactor The register's SyncInSkipFactor field. + * \param[out] reserved1 The register's RESERVED1 field. + * \param[out] syncOutMode The register's SyncOutMode field. + * \param[out] syncOutPolarity The register's SyncOutPolarity field. + * \param[out] syncOutSkipFactor The register's SyncOutSkipFactor field. + * \param[out] syncOutPulseWidth The register's SyncOutPulseWidth field. + * \param[out] reserved2 The register's RESERVED2 field. + */ +void VnUartPacket_parseSynchronizationControl(VnUartPacket *packet, uint8_t* syncInMode, uint8_t* syncInEdge, uint16_t* syncInSkipFactor, uint32_t* reserved1, uint8_t* syncOutMode, uint8_t* syncOutPolarity, uint16_t* syncOutSkipFactor, uint32_t* syncOutPulseWidth, uint32_t* reserved2); + +/** \brief Parses a response from reading the Synchronization Control register. + * + * \param[in] packet The associated packet. + * \param[out] syncInMode The register's SyncInMode field. + * \param[out] syncInEdge The register's SyncInEdge field. + * \param[out] syncInSkipFactor The register's SyncInSkipFactor field. + * \param[out] reserved1 The register's RESERVED1 field. + * \param[out] syncOutMode The register's SyncOutMode field. + * \param[out] syncOutPolarity The register's SyncOutPolarity field. + * \param[out] syncOutSkipFactor The register's SyncOutSkipFactor field. + * \param[out] syncOutPulseWidth The register's SyncOutPulseWidth field. + * \param[out] reserved2 The register's RESERVED2 field. + */ +void VnUartPacket_parseSynchronizationControlRaw(char *packet, uint8_t* syncInMode, uint8_t* syncInEdge, uint16_t* syncInSkipFactor, uint32_t* reserved1, uint8_t* syncOutMode, uint8_t* syncOutPolarity, uint16_t* syncOutSkipFactor, uint32_t* syncOutPulseWidth, uint32_t* reserved2); + +/** \brief Parses a response from reading the Synchronization Status register. + * + * \param[in] packet The associated packet. + * \param[out] syncInCount The register's SyncInCount field. + * \param[out] syncInTime The register's SyncInTime field. + * \param[out] syncOutCount The register's SyncOutCount field. + */ +void VnUartPacket_parseSynchronizationStatus(VnUartPacket *packet, uint32_t* syncInCount, uint32_t* syncInTime, uint32_t* syncOutCount); + +/** \brief Parses a response from reading the Synchronization Status register. + * + * \param[in] packet The associated packet. + * \param[out] syncInCount The register's SyncInCount field. + * \param[out] syncInTime The register's SyncInTime field. + * \param[out] syncOutCount The register's SyncOutCount field. + */ +void VnUartPacket_parseSynchronizationStatusRaw(char *packet, uint32_t* syncInCount, uint32_t* syncInTime, uint32_t* syncOutCount); + +/** \brief Parses a response from reading the Filter Basic Control register. + * + * \param[in] packet The associated packet. + * \param[out] magMode The register's MagMode field. + * \param[out] extMagMode The register's ExtMagMode field. + * \param[out] extAccMode The register's ExtAccMode field. + * \param[out] extGyroMode The register's ExtGyroMode field. + * \param[out] gyroLimit The register's GyroLimit field. + */ +void VnUartPacket_parseFilterBasicControl(VnUartPacket *packet, uint8_t* magMode, uint8_t* extMagMode, uint8_t* extAccMode, uint8_t* extGyroMode, vec3f* gyroLimit); + +/** \brief Parses a response from reading the Filter Basic Control register. + * + * \param[in] packet The associated packet. + * \param[out] magMode The register's MagMode field. + * \param[out] extMagMode The register's ExtMagMode field. + * \param[out] extAccMode The register's ExtAccMode field. + * \param[out] extGyroMode The register's ExtGyroMode field. + * \param[out] gyroLimit The register's GyroLimit field. + */ +void VnUartPacket_parseFilterBasicControlRaw(char *packet, uint8_t* magMode, uint8_t* extMagMode, uint8_t* extAccMode, uint8_t* extGyroMode, vec3f* gyroLimit); + +/** \brief Parses a response from reading the VPE Basic Control register. + * + * \param[in] packet The associated packet. + * \param[out] enable The register's Enable field. + * \param[out] headingMode The register's HeadingMode field. + * \param[out] filteringMode The register's FilteringMode field. + * \param[out] tuningMode The register's TuningMode field. + */ +void VnUartPacket_parseVpeBasicControl(VnUartPacket *packet, uint8_t* enable, uint8_t* headingMode, uint8_t* filteringMode, uint8_t* tuningMode); + +/** \brief Parses a response from reading the VPE Basic Control register. + * + * \param[in] packet The associated packet. + * \param[out] enable The register's Enable field. + * \param[out] headingMode The register's HeadingMode field. + * \param[out] filteringMode The register's FilteringMode field. + * \param[out] tuningMode The register's TuningMode field. + */ +void VnUartPacket_parseVpeBasicControlRaw(char *packet, uint8_t* enable, uint8_t* headingMode, uint8_t* filteringMode, uint8_t* tuningMode); + +/** \brief Parses a response from reading the VPE Magnetometer Basic Tuning register. + * + * \param[in] packet The associated packet. + * \param[out] baseTuning The register's BaseTuning field. + * \param[out] adaptiveTuning The register's AdaptiveTuning field. + * \param[out] adaptiveFiltering The register's AdaptiveFiltering field. + */ +void VnUartPacket_parseVpeMagnetometerBasicTuning(VnUartPacket *packet, vec3f* baseTuning, vec3f* adaptiveTuning, vec3f* adaptiveFiltering); + +/** \brief Parses a response from reading the VPE Magnetometer Basic Tuning register. + * + * \param[in] packet The associated packet. + * \param[out] baseTuning The register's BaseTuning field. + * \param[out] adaptiveTuning The register's AdaptiveTuning field. + * \param[out] adaptiveFiltering The register's AdaptiveFiltering field. + */ +void VnUartPacket_parseVpeMagnetometerBasicTuningRaw(char *packet, vec3f* baseTuning, vec3f* adaptiveTuning, vec3f* adaptiveFiltering); + +/** \brief Parses a response from reading the VPE Magnetometer Advanced Tuning register. + * + * \param[in] packet The associated packet. + * \param[out] minFiltering The register's MinFiltering field. + * \param[out] maxFiltering The register's MaxFiltering field. + * \param[out] maxAdaptRate The register's MaxAdaptRate field. + * \param[out] disturbanceWindow The register's DisturbanceWindow field. + * \param[out] maxTuning The register's MaxTuning field. + */ +void VnUartPacket_parseVpeMagnetometerAdvancedTuning(VnUartPacket *packet, vec3f* minFiltering, vec3f* maxFiltering, float* maxAdaptRate, float* disturbanceWindow, float* maxTuning); + +/** \brief Parses a response from reading the VPE Magnetometer Advanced Tuning register. + * + * \param[in] packet The associated packet. + * \param[out] minFiltering The register's MinFiltering field. + * \param[out] maxFiltering The register's MaxFiltering field. + * \param[out] maxAdaptRate The register's MaxAdaptRate field. + * \param[out] disturbanceWindow The register's DisturbanceWindow field. + * \param[out] maxTuning The register's MaxTuning field. + */ +void VnUartPacket_parseVpeMagnetometerAdvancedTuningRaw(char *packet, vec3f* minFiltering, vec3f* maxFiltering, float* maxAdaptRate, float* disturbanceWindow, float* maxTuning); + +/** \brief Parses a response from reading the VPE Accelerometer Basic Tuning register. + * + * \param[in] packet The associated packet. + * \param[out] baseTuning The register's BaseTuning field. + * \param[out] adaptiveTuning The register's AdaptiveTuning field. + * \param[out] adaptiveFiltering The register's AdaptiveFiltering field. + */ +void VnUartPacket_parseVpeAccelerometerBasicTuning(VnUartPacket *packet, vec3f* baseTuning, vec3f* adaptiveTuning, vec3f* adaptiveFiltering); + +/** \brief Parses a response from reading the VPE Accelerometer Basic Tuning register. + * + * \param[in] packet The associated packet. + * \param[out] baseTuning The register's BaseTuning field. + * \param[out] adaptiveTuning The register's AdaptiveTuning field. + * \param[out] adaptiveFiltering The register's AdaptiveFiltering field. + */ +void VnUartPacket_parseVpeAccelerometerBasicTuningRaw(char *packet, vec3f* baseTuning, vec3f* adaptiveTuning, vec3f* adaptiveFiltering); + +/** \brief Parses a response from reading the VPE Accelerometer Advanced Tuning register. + * + * \param[in] packet The associated packet. + * \param[out] minFiltering The register's MinFiltering field. + * \param[out] maxFiltering The register's MaxFiltering field. + * \param[out] maxAdaptRate The register's MaxAdaptRate field. + * \param[out] disturbanceWindow The register's DisturbanceWindow field. + * \param[out] maxTuning The register's MaxTuning field. + */ +void VnUartPacket_parseVpeAccelerometerAdvancedTuning(VnUartPacket *packet, vec3f* minFiltering, vec3f* maxFiltering, float* maxAdaptRate, float* disturbanceWindow, float* maxTuning); + +/** \brief Parses a response from reading the VPE Accelerometer Advanced Tuning register. + * + * \param[in] packet The associated packet. + * \param[out] minFiltering The register's MinFiltering field. + * \param[out] maxFiltering The register's MaxFiltering field. + * \param[out] maxAdaptRate The register's MaxAdaptRate field. + * \param[out] disturbanceWindow The register's DisturbanceWindow field. + * \param[out] maxTuning The register's MaxTuning field. + */ +void VnUartPacket_parseVpeAccelerometerAdvancedTuningRaw(char *packet, vec3f* minFiltering, vec3f* maxFiltering, float* maxAdaptRate, float* disturbanceWindow, float* maxTuning); + +/** \brief Parses a response from reading the VPE Gyro Basic Tuning register. + * + * \param[in] packet The associated packet. + * \param[out] angularWalkVariance The register's AngularWalkVariance field. + * \param[out] baseTuning The register's BaseTuning field. + * \param[out] adaptiveTuning The register's AdaptiveTuning field. + */ +void VnUartPacket_parseVpeGyroBasicTuning(VnUartPacket *packet, vec3f* angularWalkVariance, vec3f* baseTuning, vec3f* adaptiveTuning); + +/** \brief Parses a response from reading the VPE Gyro Basic Tuning register. + * + * \param[in] packet The associated packet. + * \param[out] angularWalkVariance The register's AngularWalkVariance field. + * \param[out] baseTuning The register's BaseTuning field. + * \param[out] adaptiveTuning The register's AdaptiveTuning field. + */ +void VnUartPacket_parseVpeGyroBasicTuningRaw(char *packet, vec3f* angularWalkVariance, vec3f* baseTuning, vec3f* adaptiveTuning); + +/** \brief Parses a response from reading the Filter Startup Gyro Bias register. + * + * \param[in] packet The associated packet. + * \param[out] bias The register's Bias field. + */ +void VnUartPacket_parseFilterStartupGyroBias(VnUartPacket *packet, vec3f* bias); + +/** \brief Parses a response from reading the Filter Startup Gyro Bias register. + * + * \param[in] packet The associated packet. + * \param[out] bias The register's Bias field. + */ +void VnUartPacket_parseFilterStartupGyroBiasRaw(char *packet, vec3f* bias); + +/** \brief Parses a response from reading the Magnetometer Calibration Control register. + * + * \param[in] packet The associated packet. + * \param[out] hsiMode The register's HSIMode field. + * \param[out] hsiOutput The register's HSIOutput field. + * \param[out] convergeRate The register's ConvergeRate field. + */ +void VnUartPacket_parseMagnetometerCalibrationControl(VnUartPacket *packet, uint8_t* hsiMode, uint8_t* hsiOutput, uint8_t* convergeRate); + +/** \brief Parses a response from reading the Magnetometer Calibration Control register. + * + * \param[in] packet The associated packet. + * \param[out] hsiMode The register's HSIMode field. + * \param[out] hsiOutput The register's HSIOutput field. + * \param[out] convergeRate The register's ConvergeRate field. + */ +void VnUartPacket_parseMagnetometerCalibrationControlRaw(char *packet, uint8_t* hsiMode, uint8_t* hsiOutput, uint8_t* convergeRate); + +/** \brief Parses a response from reading the Calculated Magnetometer Calibration register. + * + * \param[in] packet The associated packet. + * \param[out] c The register's C field. + * \param[out] b The register's B field. + */ +void VnUartPacket_parseCalculatedMagnetometerCalibration(VnUartPacket *packet, mat3f* c, vec3f* b); + +/** \brief Parses a response from reading the Calculated Magnetometer Calibration register. + * + * \param[in] packet The associated packet. + * \param[out] c The register's C field. + * \param[out] b The register's B field. + */ +void VnUartPacket_parseCalculatedMagnetometerCalibrationRaw(char *packet, mat3f* c, vec3f* b); + +/** \brief Parses a response from reading the Indoor Heading Mode Control register. + * + * \param[in] packet The associated packet. + * \param[out] maxRateError The register's Max Rate Error field. + * \param[out] reserved1 The register's Reserved1 field. + */ +void VnUartPacket_parseIndoorHeadingModeControl(VnUartPacket *packet, float* maxRateError, uint8_t* reserved1); + +/** \brief Parses a response from reading the Indoor Heading Mode Control register. + * + * \param[in] packet The associated packet. + * \param[out] maxRateError The register's Max Rate Error field. + * \param[out] reserved1 The register's Reserved1 field. + */ +void VnUartPacket_parseIndoorHeadingModeControlRaw(char *packet, float* maxRateError, uint8_t* reserved1); + +/** \brief Parses a response from reading the Velocity Compensation Measurement register. + * + * \param[in] packet The associated packet. + * \param[out] velocity The register's Velocity field. + */ +void VnUartPacket_parseVelocityCompensationMeasurement(VnUartPacket *packet, vec3f* velocity); + +/** \brief Parses a response from reading the Velocity Compensation Measurement register. + * + * \param[in] packet The associated packet. + * \param[out] velocity The register's Velocity field. + */ +void VnUartPacket_parseVelocityCompensationMeasurementRaw(char *packet, vec3f* velocity); + +/** \brief Parses a response from reading the Velocity Compensation Control register. + * + * \param[in] packet The associated packet. + * \param[out] mode The register's Mode field. + * \param[out] velocityTuning The register's VelocityTuning field. + * \param[out] rateTuning The register's RateTuning field. + */ +void VnUartPacket_parseVelocityCompensationControl(VnUartPacket *packet, uint8_t* mode, float* velocityTuning, float* rateTuning); + +/** \brief Parses a response from reading the Velocity Compensation Control register. + * + * \param[in] packet The associated packet. + * \param[out] mode The register's Mode field. + * \param[out] velocityTuning The register's VelocityTuning field. + * \param[out] rateTuning The register's RateTuning field. + */ +void VnUartPacket_parseVelocityCompensationControlRaw(char *packet, uint8_t* mode, float* velocityTuning, float* rateTuning); + +/** \brief Parses a response from reading the Velocity Compensation Status register. + * + * \param[in] packet The associated packet. + * \param[out] x The register's x field. + * \param[out] xDot The register's xDot field. + * \param[out] accelOffset The register's accelOffset field. + * \param[out] omega The register's omega field. + */ +void VnUartPacket_parseVelocityCompensationStatus(VnUartPacket *packet, float* x, float* xDot, vec3f* accelOffset, vec3f* omega); + +/** \brief Parses a response from reading the Velocity Compensation Status register. + * + * \param[in] packet The associated packet. + * \param[out] x The register's x field. + * \param[out] xDot The register's xDot field. + * \param[out] accelOffset The register's accelOffset field. + * \param[out] omega The register's omega field. + */ +void VnUartPacket_parseVelocityCompensationStatusRaw(char *packet, float* x, float* xDot, vec3f* accelOffset, vec3f* omega); + +/** \brief Parses a response from reading the IMU Measurements register. + * + * \param[in] packet The associated packet. + * \param[out] mag The register's Mag field. + * \param[out] accel The register's Accel field. + * \param[out] gyro The register's Gyro field. + * \param[out] temp The register's Temp field. + * \param[out] pressure The register's Pressure field. + */ +void VnUartPacket_parseImuMeasurements(VnUartPacket *packet, vec3f* mag, vec3f* accel, vec3f* gyro, float* temp, float* pressure); + +/** \brief Parses a response from reading the IMU Measurements register. + * + * \param[in] packet The associated packet. + * \param[out] mag The register's Mag field. + * \param[out] accel The register's Accel field. + * \param[out] gyro The register's Gyro field. + * \param[out] temp The register's Temp field. + * \param[out] pressure The register's Pressure field. + */ +void VnUartPacket_parseImuMeasurementsRaw(char *packet, vec3f* mag, vec3f* accel, vec3f* gyro, float* temp, float* pressure); + +/** \brief Parses a response from reading the GPS Configuration register. + * + * \param[in] packet The associated packet. + * \param[out] mode The register's Mode field. + * \param[out] ppsSource The register's PpsSource field. + * \param[out] reserved1 The register's Reserved1 field. + * \param[out] reserved2 The register's Reserved2 field. + * \param[out] reserved3 The register's Reserved3 field. + */ +void VnUartPacket_parseGpsConfiguration(VnUartPacket *packet, uint8_t* mode, uint8_t* ppsSource, uint8_t* reserved1, uint8_t* reserved2, uint8_t* reserved3); + +/** \brief Parses a response from reading the GPS Configuration register. + * + * \param[in] packet The associated packet. + * \param[out] mode The register's Mode field. + * \param[out] ppsSource The register's PpsSource field. + * \param[out] reserved1 The register's Reserved1 field. + * \param[out] reserved2 The register's Reserved2 field. + * \param[out] reserved3 The register's Reserved3 field. + */ +void VnUartPacket_parseGpsConfigurationRaw(char *packet, uint8_t* mode, uint8_t* ppsSource, uint8_t* reserved1, uint8_t* reserved2, uint8_t* reserved3); + +/** \brief Parses a response from reading the GPS Antenna Offset register. + * + * \param[in] packet The associated packet. + * \param[out] position The register's Position field. + */ +void VnUartPacket_parseGpsAntennaOffset(VnUartPacket *packet, vec3f* position); + +/** \brief Parses a response from reading the GPS Antenna Offset register. + * + * \param[in] packet The associated packet. + * \param[out] position The register's Position field. + */ +void VnUartPacket_parseGpsAntennaOffsetRaw(char *packet, vec3f* position); + +/** \brief Parses a response from reading the GPS Solution - LLA register. + * + * \param[in] packet The associated packet. + * \param[out] time The register's Time field. + * \param[out] week The register's Week field. + * \param[out] gpsFix The register's GpsFix field. + * \param[out] numSats The register's NumSats field. + * \param[out] lla The register's Lla field. + * \param[out] nedVel The register's NedVel field. + * \param[out] nedAcc The register's NedAcc field. + * \param[out] speedAcc The register's SpeedAcc field. + * \param[out] timeAcc The register's TimeAcc field. + */ +void VnUartPacket_parseGpsSolutionLla(VnUartPacket *packet, double* time, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, vec3d* lla, vec3f* nedVel, vec3f* nedAcc, float* speedAcc, float* timeAcc); + +/** \brief Parses a response from reading the GPS Solution - LLA register. + * + * \param[in] packet The associated packet. + * \param[out] time The register's Time field. + * \param[out] week The register's Week field. + * \param[out] gpsFix The register's GpsFix field. + * \param[out] numSats The register's NumSats field. + * \param[out] lla The register's Lla field. + * \param[out] nedVel The register's NedVel field. + * \param[out] nedAcc The register's NedAcc field. + * \param[out] speedAcc The register's SpeedAcc field. + * \param[out] timeAcc The register's TimeAcc field. + */ +void VnUartPacket_parseGpsSolutionLlaRaw(char *packet, double* time, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, vec3d* lla, vec3f* nedVel, vec3f* nedAcc, float* speedAcc, float* timeAcc); + +/** \brief Parses a response from reading the GPS Solution - ECEF register. + * + * \param[in] packet The associated packet. + * \param[out] tow The register's Tow field. + * \param[out] week The register's Week field. + * \param[out] gpsFix The register's GpsFix field. + * \param[out] numSats The register's NumSats field. + * \param[out] position The register's Position field. + * \param[out] velocity The register's Velocity field. + * \param[out] posAcc The register's PosAcc field. + * \param[out] speedAcc The register's SpeedAcc field. + * \param[out] timeAcc The register's TimeAcc field. + */ +void VnUartPacket_parseGpsSolutionEcef(VnUartPacket *packet, double* tow, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, vec3d* position, vec3f* velocity, vec3f* posAcc, float* speedAcc, float* timeAcc); + +/** \brief Parses a response from reading the GPS Solution - ECEF register. + * + * \param[in] packet The associated packet. + * \param[out] tow The register's Tow field. + * \param[out] week The register's Week field. + * \param[out] gpsFix The register's GpsFix field. + * \param[out] numSats The register's NumSats field. + * \param[out] position The register's Position field. + * \param[out] velocity The register's Velocity field. + * \param[out] posAcc The register's PosAcc field. + * \param[out] speedAcc The register's SpeedAcc field. + * \param[out] timeAcc The register's TimeAcc field. + */ +void VnUartPacket_parseGpsSolutionEcefRaw(char *packet, double* tow, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, vec3d* position, vec3f* velocity, vec3f* posAcc, float* speedAcc, float* timeAcc); + +/** \brief Parses a response from reading the INS Solution - LLA register. + * + * \param[in] packet The associated packet. + * \param[out] time The register's Time field. + * \param[out] week The register's Week field. + * \param[out] status The register's Status field. + * \param[out] yawPitchRoll The register's YawPitchRoll field. + * \param[out] position The register's Position field. + * \param[out] nedVel The register's NedVel field. + * \param[out] attUncertainty The register's AttUncertainty field. + * \param[out] posUncertainty The register's PosUncertainty field. + * \param[out] velUncertainty The register's VelUncertainty field. + */ +void VnUartPacket_parseInsSolutionLla(VnUartPacket *packet, double* time, uint16_t* week, uint16_t* status, vec3f* yawPitchRoll, vec3d* position, vec3f* nedVel, float* attUncertainty, float* posUncertainty, float* velUncertainty); + +/** \brief Parses a response from reading the INS Solution - LLA register. + * + * \param[in] packet The associated packet. + * \param[out] time The register's Time field. + * \param[out] week The register's Week field. + * \param[out] status The register's Status field. + * \param[out] yawPitchRoll The register's YawPitchRoll field. + * \param[out] position The register's Position field. + * \param[out] nedVel The register's NedVel field. + * \param[out] attUncertainty The register's AttUncertainty field. + * \param[out] posUncertainty The register's PosUncertainty field. + * \param[out] velUncertainty The register's VelUncertainty field. + */ +void VnUartPacket_parseInsSolutionLlaRaw(char *packet, double* time, uint16_t* week, uint16_t* status, vec3f* yawPitchRoll, vec3d* position, vec3f* nedVel, float* attUncertainty, float* posUncertainty, float* velUncertainty); + +/** \brief Parses a response from reading the INS Solution - ECEF register. + * + * \param[in] packet The associated packet. + * \param[out] time The register's Time field. + * \param[out] week The register's Week field. + * \param[out] status The register's Status field. + * \param[out] yawPitchRoll The register's YawPitchRoll field. + * \param[out] position The register's Position field. + * \param[out] velocity The register's Velocity field. + * \param[out] attUncertainty The register's AttUncertainty field. + * \param[out] posUncertainty The register's PosUncertainty field. + * \param[out] velUncertainty The register's VelUncertainty field. + */ +void VnUartPacket_parseInsSolutionEcef(VnUartPacket *packet, double* time, uint16_t* week, uint16_t* status, vec3f* yawPitchRoll, vec3d* position, vec3f* velocity, float* attUncertainty, float* posUncertainty, float* velUncertainty); + +/** \brief Parses a response from reading the INS Solution - ECEF register. + * + * \param[in] packet The associated packet. + * \param[out] time The register's Time field. + * \param[out] week The register's Week field. + * \param[out] status The register's Status field. + * \param[out] yawPitchRoll The register's YawPitchRoll field. + * \param[out] position The register's Position field. + * \param[out] velocity The register's Velocity field. + * \param[out] attUncertainty The register's AttUncertainty field. + * \param[out] posUncertainty The register's PosUncertainty field. + * \param[out] velUncertainty The register's VelUncertainty field. + */ +void VnUartPacket_parseInsSolutionEcefRaw(char *packet, double* time, uint16_t* week, uint16_t* status, vec3f* yawPitchRoll, vec3d* position, vec3f* velocity, float* attUncertainty, float* posUncertainty, float* velUncertainty); + +/** \brief Parses a response from reading the INS Basic Configuration register. + * + * \param[in] packet The associated packet. + * \param[out] scenario The register's Scenario field. + * \param[out] ahrsAiding The register's AhrsAiding field. + * \param[out] estBaseline The register's EstBaseline field. + * \param[out] resv2 The register's Resv2 field. + */ +void VnUartPacket_parseInsBasicConfiguration(VnUartPacket *packet, uint8_t* scenario, uint8_t* ahrsAiding, uint8_t* estBaseline, uint8_t* resv2); + +/** \brief Parses a response from reading the INS Basic Configuration register. + * + * \param[in] packet The associated packet. + * \param[out] scenario The register's Scenario field. + * \param[out] ahrsAiding The register's AhrsAiding field. + * \param[out] estBaseline The register's EstBaseline field. + * \param[out] resv2 The register's Resv2 field. + */ +void VnUartPacket_parseInsBasicConfigurationRaw(char *packet, uint8_t* scenario, uint8_t* ahrsAiding, uint8_t* estBaseline, uint8_t* resv2); + +/** \brief Parses a response from reading the INS Advanced Configuration register. + * + * \param[in] packet The associated packet. + * \param[out] useMag The register's UseMag field. + * \param[out] usePres The register's UsePres field. + * \param[out] posAtt The register's PosAtt field. + * \param[out] velAtt The register's VelAtt field. + * \param[out] velBias The register's VelBias field. + * \param[out] useFoam The register's UseFoam field. + * \param[out] gpsCovType The register's GPSCovType field. + * \param[out] velCount The register's VelCount field. + * \param[out] velInit The register's VelInit field. + * \param[out] moveOrigin The register's MoveOrigin field. + * \param[out] gpsTimeout The register's GPSTimeout field. + * \param[out] deltaLimitPos The register's DeltaLimitPos field. + * \param[out] deltaLimitVel The register's DeltaLimitVel field. + * \param[out] minPosUncertainty The register's MinPosUncertainty field. + * \param[out] minVelUncertainty The register's MinVelUncertainty field. + */ +void VnUartPacket_parseInsAdvancedConfiguration(VnUartPacket *packet, uint8_t* useMag, uint8_t* usePres, uint8_t* posAtt, uint8_t* velAtt, uint8_t* velBias, uint8_t* useFoam, uint8_t* gpsCovType, uint8_t* velCount, float* velInit, float* moveOrigin, float* gpsTimeout, float* deltaLimitPos, float* deltaLimitVel, float* minPosUncertainty, float* minVelUncertainty); + +/** \brief Parses a response from reading the INS Advanced Configuration register. + * + * \param[in] packet The associated packet. + * \param[out] useMag The register's UseMag field. + * \param[out] usePres The register's UsePres field. + * \param[out] posAtt The register's PosAtt field. + * \param[out] velAtt The register's VelAtt field. + * \param[out] velBias The register's VelBias field. + * \param[out] useFoam The register's UseFoam field. + * \param[out] gpsCovType The register's GPSCovType field. + * \param[out] velCount The register's VelCount field. + * \param[out] velInit The register's VelInit field. + * \param[out] moveOrigin The register's MoveOrigin field. + * \param[out] gpsTimeout The register's GPSTimeout field. + * \param[out] deltaLimitPos The register's DeltaLimitPos field. + * \param[out] deltaLimitVel The register's DeltaLimitVel field. + * \param[out] minPosUncertainty The register's MinPosUncertainty field. + * \param[out] minVelUncertainty The register's MinVelUncertainty field. + */ +void VnUartPacket_parseInsAdvancedConfigurationRaw(char *packet, uint8_t* useMag, uint8_t* usePres, uint8_t* posAtt, uint8_t* velAtt, uint8_t* velBias, uint8_t* useFoam, uint8_t* gpsCovType, uint8_t* velCount, float* velInit, float* moveOrigin, float* gpsTimeout, float* deltaLimitPos, float* deltaLimitVel, float* minPosUncertainty, float* minVelUncertainty); + +/** \brief Parses a response from reading the INS State - LLA register. + * + * \param[in] packet The associated packet. + * \param[out] yawPitchRoll The register's YawPitchRoll field. + * \param[out] position The register's Position field. + * \param[out] velocity The register's Velocity field. + * \param[out] accel The register's Accel field. + * \param[out] angularRate The register's AngularRate field. + */ +void VnUartPacket_parseInsStateLla(VnUartPacket *packet, vec3f* yawPitchRoll, vec3d* position, vec3f* velocity, vec3f* accel, vec3f* angularRate); + +/** \brief Parses a response from reading the INS State - LLA register. + * + * \param[in] packet The associated packet. + * \param[out] yawPitchRoll The register's YawPitchRoll field. + * \param[out] position The register's Position field. + * \param[out] velocity The register's Velocity field. + * \param[out] accel The register's Accel field. + * \param[out] angularRate The register's AngularRate field. + */ +void VnUartPacket_parseInsStateLlaRaw(char *packet, vec3f* yawPitchRoll, vec3d* position, vec3f* velocity, vec3f* accel, vec3f* angularRate); + +/** \brief Parses a response from reading the INS State - ECEF register. + * + * \param[in] packet The associated packet. + * \param[out] yawPitchRoll The register's YawPitchRoll field. + * \param[out] position The register's Position field. + * \param[out] velocity The register's Velocity field. + * \param[out] accel The register's Accel field. + * \param[out] angularRate The register's AngularRate field. + */ +void VnUartPacket_parseInsStateEcef(VnUartPacket *packet, vec3f* yawPitchRoll, vec3d* position, vec3f* velocity, vec3f* accel, vec3f* angularRate); + +/** \brief Parses a response from reading the INS State - ECEF register. + * + * \param[in] packet The associated packet. + * \param[out] yawPitchRoll The register's YawPitchRoll field. + * \param[out] position The register's Position field. + * \param[out] velocity The register's Velocity field. + * \param[out] accel The register's Accel field. + * \param[out] angularRate The register's AngularRate field. + */ +void VnUartPacket_parseInsStateEcefRaw(char *packet, vec3f* yawPitchRoll, vec3d* position, vec3f* velocity, vec3f* accel, vec3f* angularRate); + +/** \brief Parses a response from reading the Startup Filter Bias Estimate register. + * + * \param[in] packet The associated packet. + * \param[out] gyroBias The register's GyroBias field. + * \param[out] accelBias The register's AccelBias field. + * \param[out] pressureBias The register's PressureBias field. + */ +void VnUartPacket_parseStartupFilterBiasEstimate(VnUartPacket *packet, vec3f* gyroBias, vec3f* accelBias, float* pressureBias); + +/** \brief Parses a response from reading the Startup Filter Bias Estimate register. + * + * \param[in] packet The associated packet. + * \param[out] gyroBias The register's GyroBias field. + * \param[out] accelBias The register's AccelBias field. + * \param[out] pressureBias The register's PressureBias field. + */ +void VnUartPacket_parseStartupFilterBiasEstimateRaw(char *packet, vec3f* gyroBias, vec3f* accelBias, float* pressureBias); + +/** \brief Parses a response from reading the Delta Theta and Delta Velocity register. + * + * \param[in] packet The associated packet. + * \param[out] deltaTime The register's DeltaTime field. + * \param[out] deltaTheta The register's DeltaTheta field. + * \param[out] deltaVelocity The register's DeltaVelocity field. + */ +void VnUartPacket_parseDeltaThetaAndDeltaVelocity(VnUartPacket *packet, float* deltaTime, vec3f* deltaTheta, vec3f* deltaVelocity); + +/** \brief Parses a response from reading the Delta Theta and Delta Velocity register. + * + * \param[in] packet The associated packet. + * \param[out] deltaTime The register's DeltaTime field. + * \param[out] deltaTheta The register's DeltaTheta field. + * \param[out] deltaVelocity The register's DeltaVelocity field. + */ +void VnUartPacket_parseDeltaThetaAndDeltaVelocityRaw(char *packet, float* deltaTime, vec3f* deltaTheta, vec3f* deltaVelocity); + +/** \brief Parses a response from reading the Delta Theta and Delta Velocity Configuration register. + * + * \param[in] packet The associated packet. + * \param[out] integrationFrame The register's IntegrationFrame field. + * \param[out] gyroCompensation The register's GyroCompensation field. + * \param[out] accelCompensation The register's AccelCompensation field. + * \param[out] reserved1 The register's Reserved1 field. + * \param[out] reserved2 The register's Reserved2 field. + */ +void VnUartPacket_parseDeltaThetaAndDeltaVelocityConfiguration(VnUartPacket *packet, uint8_t* integrationFrame, uint8_t* gyroCompensation, uint8_t* accelCompensation, uint8_t* reserved1, uint16_t* reserved2); + +/** \brief Parses a response from reading the Delta Theta and Delta Velocity Configuration register. + * + * \param[in] packet The associated packet. + * \param[out] integrationFrame The register's IntegrationFrame field. + * \param[out] gyroCompensation The register's GyroCompensation field. + * \param[out] accelCompensation The register's AccelCompensation field. + * \param[out] reserved1 The register's Reserved1 field. + * \param[out] reserved2 The register's Reserved2 field. + */ +void VnUartPacket_parseDeltaThetaAndDeltaVelocityConfigurationRaw(char *packet, uint8_t* integrationFrame, uint8_t* gyroCompensation, uint8_t* accelCompensation, uint8_t* reserved1, uint16_t* reserved2); + +/** \brief Parses a response from reading the Reference Vector Configuration register. + * + * \param[in] packet The associated packet. + * \param[out] useMagModel The register's UseMagModel field. + * \param[out] useGravityModel The register's UseGravityModel field. + * \param[out] resv1 The register's Resv1 field. + * \param[out] resv2 The register's Resv2 field. + * \param[out] recalcThreshold The register's RecalcThreshold field. + * \param[out] year The register's Year field. + * \param[out] position The register's Position field. + */ +void VnUartPacket_parseReferenceVectorConfiguration(VnUartPacket *packet, uint8_t* useMagModel, uint8_t* useGravityModel, uint8_t* resv1, uint8_t* resv2, uint32_t* recalcThreshold, float* year, vec3d* position); + +/** \brief Parses a response from reading the Reference Vector Configuration register. + * + * \param[in] packet The associated packet. + * \param[out] useMagModel The register's UseMagModel field. + * \param[out] useGravityModel The register's UseGravityModel field. + * \param[out] resv1 The register's Resv1 field. + * \param[out] resv2 The register's Resv2 field. + * \param[out] recalcThreshold The register's RecalcThreshold field. + * \param[out] year The register's Year field. + * \param[out] position The register's Position field. + */ +void VnUartPacket_parseReferenceVectorConfigurationRaw(char *packet, uint8_t* useMagModel, uint8_t* useGravityModel, uint8_t* resv1, uint8_t* resv2, uint32_t* recalcThreshold, float* year, vec3d* position); + +/** \brief Parses a response from reading the Gyro Compensation register. + * + * \param[in] packet The associated packet. + * \param[out] c The register's C field. + * \param[out] b The register's B field. + */ +void VnUartPacket_parseGyroCompensation(VnUartPacket *packet, mat3f* c, vec3f* b); + +/** \brief Parses a response from reading the Gyro Compensation register. + * + * \param[in] packet The associated packet. + * \param[out] c The register's C field. + * \param[out] b The register's B field. + */ +void VnUartPacket_parseGyroCompensationRaw(char *packet, mat3f* c, vec3f* b); + +/** \brief Parses a response from reading the IMU Filtering Configuration register. + * + * \param[in] packet The associated packet. + * \param[out] magWindowSize The register's MagWindowSize field. + * \param[out] accelWindowSize The register's AccelWindowSize field. + * \param[out] gyroWindowSize The register's GyroWindowSize field. + * \param[out] tempWindowSize The register's TempWindowSize field. + * \param[out] presWindowSize The register's PresWindowSize field. + * \param[out] magFilterMode The register's MagFilterMode field. + * \param[out] accelFilterMode The register's AccelFilterMode field. + * \param[out] gyroFilterMode The register's GyroFilterMode field. + * \param[out] tempFilterMode The register's TempFilterMode field. + * \param[out] presFilterMode The register's PresFilterMode field. + */ +void VnUartPacket_parseImuFilteringConfiguration(VnUartPacket *packet, uint16_t* magWindowSize, uint16_t* accelWindowSize, uint16_t* gyroWindowSize, uint16_t* tempWindowSize, uint16_t* presWindowSize, uint8_t* magFilterMode, uint8_t* accelFilterMode, uint8_t* gyroFilterMode, uint8_t* tempFilterMode, uint8_t* presFilterMode); + +/** \brief Parses a response from reading the IMU Filtering Configuration register. + * + * \param[in] packet The associated packet. + * \param[out] magWindowSize The register's MagWindowSize field. + * \param[out] accelWindowSize The register's AccelWindowSize field. + * \param[out] gyroWindowSize The register's GyroWindowSize field. + * \param[out] tempWindowSize The register's TempWindowSize field. + * \param[out] presWindowSize The register's PresWindowSize field. + * \param[out] magFilterMode The register's MagFilterMode field. + * \param[out] accelFilterMode The register's AccelFilterMode field. + * \param[out] gyroFilterMode The register's GyroFilterMode field. + * \param[out] tempFilterMode The register's TempFilterMode field. + * \param[out] presFilterMode The register's PresFilterMode field. + */ +void VnUartPacket_parseImuFilteringConfigurationRaw(char *packet, uint16_t* magWindowSize, uint16_t* accelWindowSize, uint16_t* gyroWindowSize, uint16_t* tempWindowSize, uint16_t* presWindowSize, uint8_t* magFilterMode, uint8_t* accelFilterMode, uint8_t* gyroFilterMode, uint8_t* tempFilterMode, uint8_t* presFilterMode); + +/** \brief Parses a response from reading the GPS Compass Baseline register. + * + * \param[in] packet The associated packet. + * \param[out] position The register's Position field. + * \param[out] uncertainty The register's Uncertainty field. + */ +void VnUartPacket_parseGpsCompassBaseline(VnUartPacket *packet, vec3f* position, vec3f* uncertainty); + +/** \brief Parses a response from reading the GPS Compass Baseline register. + * + * \param[in] packet The associated packet. + * \param[out] position The register's Position field. + * \param[out] uncertainty The register's Uncertainty field. + */ +void VnUartPacket_parseGpsCompassBaselineRaw(char *packet, vec3f* position, vec3f* uncertainty); + +/** \brief Parses a response from reading the GPS Compass Estimated Baseline register. + * + * \param[in] packet The associated packet. + * \param[out] estBaselineUsed The register's EstBaselineUsed field. + * \param[out] resv The register's Resv field. + * \param[out] numMeas The register's NumMeas field. + * \param[out] position The register's Position field. + * \param[out] uncertainty The register's Uncertainty field. + */ +void VnUartPacket_parseGpsCompassEstimatedBaseline(VnUartPacket *packet, uint8_t* estBaselineUsed, uint8_t* resv, uint16_t* numMeas, vec3f* position, vec3f* uncertainty); + +/** \brief Parses a response from reading the GPS Compass Estimated Baseline register. + * + * \param[in] packet The associated packet. + * \param[out] estBaselineUsed The register's EstBaselineUsed field. + * \param[out] resv The register's Resv field. + * \param[out] numMeas The register's NumMeas field. + * \param[out] position The register's Position field. + * \param[out] uncertainty The register's Uncertainty field. + */ +void VnUartPacket_parseGpsCompassEstimatedBaselineRaw(char *packet, uint8_t* estBaselineUsed, uint8_t* resv, uint16_t* numMeas, vec3f* position, vec3f* uncertainty); + +/** \brief Parses a response from reading the IMU Rate Configuration register. + * + * \param[in] packet The associated packet. + * \param[out] imuRate The register's imuRate field. + * \param[out] navDivisor The register's NavDivisor field. + * \param[out] filterTargetRate The register's filterTargetRate field. + * \param[out] filterMinRate The register's filterMinRate field. + */ +void VnUartPacket_parseImuRateConfiguration(VnUartPacket *packet, uint16_t* imuRate, uint16_t* navDivisor, float* filterTargetRate, float* filterMinRate); + +/** \brief Parses a response from reading the IMU Rate Configuration register. + * + * \param[in] packet The associated packet. + * \param[out] imuRate The register's imuRate field. + * \param[out] navDivisor The register's NavDivisor field. + * \param[out] filterTargetRate The register's filterTargetRate field. + * \param[out] filterMinRate The register's filterMinRate field. + */ +void VnUartPacket_parseImuRateConfigurationRaw(char *packet, uint16_t* imuRate, uint16_t* navDivisor, float* filterTargetRate, float* filterMinRate); + +/** \brief Parses a response from reading the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register. + * + * \param[in] packet The associated packet. + * \param[out] yawPitchRoll The register's YawPitchRoll field. + * \param[out] bodyAccel The register's BodyAccel field. + * \param[out] gyro The register's Gyro field. + */ +void VnUartPacket_parseYawPitchRollTrueBodyAccelerationAndAngularRates(VnUartPacket *packet, vec3f* yawPitchRoll, vec3f* bodyAccel, vec3f* gyro); + +/** \brief Parses a response from reading the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register. + * + * \param[in] packet The associated packet. + * \param[out] yawPitchRoll The register's YawPitchRoll field. + * \param[out] bodyAccel The register's BodyAccel field. + * \param[out] gyro The register's Gyro field. + */ +void VnUartPacket_parseYawPitchRollTrueBodyAccelerationAndAngularRatesRaw(char *packet, vec3f* yawPitchRoll, vec3f* bodyAccel, vec3f* gyro); + +/** \brief Parses a response from reading the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register. + * + * \param[in] packet The associated packet. + * \param[out] yawPitchRoll The register's YawPitchRoll field. + * \param[out] inertialAccel The register's InertialAccel field. + * \param[out] gyro The register's Gyro field. + */ +void VnUartPacket_parseYawPitchRollTrueInertialAccelerationAndAngularRates(VnUartPacket *packet, vec3f* yawPitchRoll, vec3f* inertialAccel, vec3f* gyro); + +/** \brief Parses a response from reading the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register. + * + * \param[in] packet The associated packet. + * \param[out] yawPitchRoll The register's YawPitchRoll field. + * \param[out] inertialAccel The register's InertialAccel field. + * \param[out] gyro The register's Gyro field. + */ +void VnUartPacket_parseYawPitchRollTrueInertialAccelerationAndAngularRatesRaw(char *packet, vec3f* yawPitchRoll, vec3f* inertialAccel, vec3f* gyro); + +/** \} */ + +/** \brief Converts a VnAsciiAsync into a string. +* +* \param[out] out The buffer to place the string in. +* \param[in] val The VnAsciiAsync value to convert to string. +* \return The converted value. */ +void strFromVnAsciiAsync(char *out, VnAsciiAsync val); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/protocol/upackf.h b/src/drivers/ins/vectornav/libvnc/include/vn/protocol/upackf.h new file mode 100644 index 0000000000..40eb35749b --- /dev/null +++ b/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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/sensors.h b/src/drivers/ins/vectornav/libvnc/include/vn/sensors.h new file mode 100644 index 0000000000..30647773c6 --- /dev/null +++ b/src/drivers/ins/vectornav/libvnc/include/vn/sensors.h @@ -0,0 +1,2020 @@ +/** \file +* {COMMON_HEADER} +* +* \section Description +* This header file contains declarations for using VectorNav sensors. +*/ +#ifndef _VNSENSORS_H_ +#define _VNSENSORS_H_ + +#include +#include + +#include "vn/int.h" +#include "vn/error.h" +#include "vn/enum.h" +#include "vn/bool.h" +#include "vn/protocol/upack.h" +#include "vn/protocol/upackf.h" +#include "vn/xplat/serialport.h" +#include "vn/xplat/event.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable : 4820) +#endif + +// Define the max record size of a firmware update file +#define MAXFIRMWAREUPDATERECORDSIZE 300 + +/** \defgroup registerStructures Register Structures + * \brief These structures represent the various registers on a VecotorNav + * sensor. + * + * \{ */ + + +/** \brief Structure representing a Binary Output register. + * + * The field outputGroup available on the sensor's register is not necessary + * in this structure since all read/writes operations will automatically + * determine this from the settings for the individual groups within this + * structure. */ +typedef struct +{ + AsyncMode asyncMode; /**< The asyncMode field. */ + uint16_t rateDivisor; /**< The rateDivisor field. */ + CommonGroup commonField; /**< Group 1 (Common) */ + TimeGroup timeField; /**< Group 2 (Time) */ + ImuGroup imuField; /**< Group 3 (IMU) */ + GpsGroup gpsField; /**< Group 4 (GPS) */ + AttitudeGroup attitudeField; /**< Group 5 (Attitude) */ + InsGroup insField; /**< Group 6 (INS) */ + GpsGroup gps2Field; /**< Group 7 (GPS2) */ +} BinaryOutputRegister; + +/** \brief Initializes a BinaryOutputRegister structure. + * + * \param[in] reg The BinaryOutputRegister structure to initialize. + * \param[in] asyncMode Value to initialize the asyncMode field with. + * \param[in] rateDivisor Value to initialize the rateDivisor field with. + * \param[in] commonField Value to initialize the commonField with. + * \param[in] timeField Value to initialize the timeField with. + * \param[in] imuField Value to initialize the imuField with. + * \param[in] gpsField Value to initialize the gpsField with. + * \param[in] attitudeField Value to initialize the attitudeField with. + * \param[in] insField Value to initialize the insField with. */ +void BinaryOutputRegister_initialize( + BinaryOutputRegister *reg, + AsyncMode asyncMode, + uint32_t rateDivisor, + CommonGroup commonField, + TimeGroup timeField, + ImuGroup imuField, + GpsGroup gpsField, + AttitudeGroup attitudeField, + InsGroup insField, + GpsGroup gps2Field); + +/** \brief Structure representing the Quaternion, Magnetic, Acceleration and Angular Rates register. */ +typedef struct +{ + /** \brief The Quat field. */ + vec4f quat; + + /** \brief The Mag field. */ + vec3f mag; + + /** \brief The Accel field. */ + vec3f accel; + + /** \brief The Gyro field. */ + vec3f gyro; + +} QuaternionMagneticAccelerationAndAngularRatesRegister; + +/** \brief Structure representing the Magnetic, Acceleration and Angular Rates register. */ +typedef struct +{ + /** \brief The Mag field. */ + vec3f mag; + + /** \brief The Accel field. */ + vec3f accel; + + /** \brief The Gyro field. */ + vec3f gyro; + +} MagneticAccelerationAndAngularRatesRegister; + +/** \brief Structure representing the Magnetic and Gravity Reference Vectors register. */ +typedef struct +{ + /** \brief The MagRef field. */ + vec3f magRef; + + /** \brief The AccRef field. */ + vec3f accRef; + +} MagneticAndGravityReferenceVectorsRegister; + +/** \brief Structure representing the Filter Measurements Variance Parameters register. */ +typedef struct +{ + /** \brief The Angular Walk Variance field. */ + float angularWalkVariance; + + /** \brief The Angular Rate Variance field. */ + vec3f angularRateVariance; + + /** \brief The Magnetic Variance field. */ + vec3f magneticVariance; + + /** \brief The Acceleration Variance field. */ + vec3f accelerationVariance; + +} FilterMeasurementsVarianceParametersRegister; + +/** \brief Structure representing the Magnetometer Compensation register. */ +typedef struct +{ + /** \brief The C field. */ + mat3f c; + + /** \brief The B field. */ + vec3f b; + +} MagnetometerCompensationRegister; + +/** \brief Structure representing the Filter Active Tuning Parameters register. */ +typedef struct +{ + /** \brief The Magnetic Disturbance Gain field. */ + float magneticDisturbanceGain; + + /** \brief The Acceleration Disturbance Gain field. */ + float accelerationDisturbanceGain; + + /** \brief The Magnetic Disturbance Memory field. */ + float magneticDisturbanceMemory; + + /** \brief The Acceleration Disturbance Memory field. */ + float accelerationDisturbanceMemory; + +} FilterActiveTuningParametersRegister; + +/** \brief Structure representing the Acceleration Compensation register. */ +typedef struct +{ + /** \brief The C field. */ + mat3f c; + + /** \brief The B field. */ + vec3f b; + +} AccelerationCompensationRegister; + +/** \brief Structure representing the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register. */ +typedef struct +{ + /** \brief The YawPitchRoll field. */ + vec3f yawPitchRoll; + + /** \brief The Mag field. */ + vec3f mag; + + /** \brief The Accel field. */ + vec3f accel; + + /** \brief The Gyro field. */ + vec3f gyro; + +} YawPitchRollMagneticAccelerationAndAngularRatesRegister; + +/** \brief Structure representing the Communication Protocol Control register. */ +typedef struct +{ + /** \brief The SerialCount field. */ + uint8_t serialCount; + + /** \brief The SerialStatus field. */ + uint8_t serialStatus; + + /** \brief The SPICount field. */ + uint8_t spiCount; + + /** \brief The SPIStatus field. */ + uint8_t spiStatus; + + /** \brief The SerialChecksum field. */ + uint8_t serialChecksum; + + /** \brief The SPIChecksum field. */ + uint8_t spiChecksum; + + /** \brief The ErrorMode field. */ + uint8_t errorMode; + +} CommunicationProtocolControlRegister; + +/** \brief Structure representing the Synchronization Control register. */ +typedef struct +{ + /** \brief The SyncInMode field. */ + uint8_t syncInMode; + + /** \brief The SyncInEdge field. */ + uint8_t syncInEdge; + + /** \brief The SyncInSkipFactor field. */ + uint16_t syncInSkipFactor; + + /** \brief The SyncOutMode field. */ + uint8_t syncOutMode; + + /** \brief The SyncOutPolarity field. */ + uint8_t syncOutPolarity; + + /** \brief The SyncOutSkipFactor field. */ + uint16_t syncOutSkipFactor; + + /** \brief The SyncOutPulseWidth field. */ + uint32_t syncOutPulseWidth; + +} SynchronizationControlRegister; + +/** \brief Structure representing the Synchronization Status register. */ +typedef struct +{ + /** \brief The SyncInCount field. */ + uint32_t syncInCount; + + /** \brief The SyncInTime field. */ + uint32_t syncInTime; + + /** \brief The SyncOutCount field. */ + uint32_t syncOutCount; + +} SynchronizationStatusRegister; + +/** \brief Structure representing the Filter Basic Control register. */ +typedef struct +{ + /** \brief The MagMode field. */ + uint8_t magMode; + + /** \brief The ExtMagMode field. */ + uint8_t extMagMode; + + /** \brief The ExtAccMode field. */ + uint8_t extAccMode; + + /** \brief The ExtGyroMode field. */ + uint8_t extGyroMode; + + /** \brief The GyroLimit field. */ + vec3f gyroLimit; + +} FilterBasicControlRegister; + +/** \brief Structure representing the VPE Basic Control register. */ +typedef struct +{ + /** \brief The Enable field. */ + uint8_t enable; + + /** \brief The HeadingMode field. */ + uint8_t headingMode; + + /** \brief The FilteringMode field. */ + uint8_t filteringMode; + + /** \brief The TuningMode field. */ + uint8_t tuningMode; + +} VpeBasicControlRegister; + +/** \brief Structure representing the VPE Magnetometer Basic Tuning register. */ +typedef struct +{ + /** \brief The BaseTuning field. */ + vec3f baseTuning; + + /** \brief The AdaptiveTuning field. */ + vec3f adaptiveTuning; + + /** \brief The AdaptiveFiltering field. */ + vec3f adaptiveFiltering; + +} VpeMagnetometerBasicTuningRegister; + +/** \brief Structure representing the VPE Magnetometer Advanced Tuning register. */ +typedef struct +{ + /** \brief The MinFiltering field. */ + vec3f minFiltering; + + /** \brief The MaxFiltering field. */ + vec3f maxFiltering; + + /** \brief The MaxAdaptRate field. */ + float maxAdaptRate; + + /** \brief The DisturbanceWindow field. */ + float disturbanceWindow; + + /** \brief The MaxTuning field. */ + float maxTuning; + +} VpeMagnetometerAdvancedTuningRegister; + +/** \brief Structure representing the VPE Accelerometer Basic Tuning register. */ +typedef struct +{ + /** \brief The BaseTuning field. */ + vec3f baseTuning; + + /** \brief The AdaptiveTuning field. */ + vec3f adaptiveTuning; + + /** \brief The AdaptiveFiltering field. */ + vec3f adaptiveFiltering; + +} VpeAccelerometerBasicTuningRegister; + +/** \brief Structure representing the VPE Accelerometer Advanced Tuning register. */ +typedef struct +{ + /** \brief The MinFiltering field. */ + vec3f minFiltering; + + /** \brief The MaxFiltering field. */ + vec3f maxFiltering; + + /** \brief The MaxAdaptRate field. */ + float maxAdaptRate; + + /** \brief The DisturbanceWindow field. */ + float disturbanceWindow; + + /** \brief The MaxTuning field. */ + float maxTuning; + +} VpeAccelerometerAdvancedTuningRegister; + +/** \brief Structure representing the VPE Gyro Basic Tuning register. */ +typedef struct +{ + /** \brief The AngularWalkVariance field. */ + vec3f angularWalkVariance; + + /** \brief The BaseTuning field. */ + vec3f baseTuning; + + /** \brief The AdaptiveTuning field. */ + vec3f adaptiveTuning; + +} VpeGyroBasicTuningRegister; + +/** \brief Structure representing the Magnetometer Calibration Control register. */ +typedef struct +{ + /** \brief The HSIMode field. */ + uint8_t hsiMode; + + /** \brief The HSIOutput field. */ + uint8_t hsiOutput; + + /** \brief The ConvergeRate field. */ + uint8_t convergeRate; + +} MagnetometerCalibrationControlRegister; + +/** \brief Structure representing the Calculated Magnetometer Calibration register. */ +typedef struct +{ + /** \brief The C field. */ + mat3f c; + + /** \brief The B field. */ + vec3f b; + +} CalculatedMagnetometerCalibrationRegister; + +/** \brief Structure representing the Velocity Compensation Control register. */ +typedef struct +{ + /** \brief The Mode field. */ + uint8_t mode; + + /** \brief The VelocityTuning field. */ + float velocityTuning; + + /** \brief The RateTuning field. */ + float rateTuning; + +} VelocityCompensationControlRegister; + +/** \brief Structure representing the Velocity Compensation Status register. */ +typedef struct +{ + /** \brief The x field. */ + float x; + + /** \brief The xDot field. */ + float xDot; + + /** \brief The accelOffset field. */ + vec3f accelOffset; + + /** \brief The omega field. */ + vec3f omega; + +} VelocityCompensationStatusRegister; + +/** \brief Structure representing the IMU Measurements register. */ +typedef struct +{ + /** \brief The Mag field. */ + vec3f mag; + + /** \brief The Accel field. */ + vec3f accel; + + /** \brief The Gyro field. */ + vec3f gyro; + + /** \brief The Temp field. */ + float temp; + + /** \brief The Pressure field. */ + float pressure; + +} ImuMeasurementsRegister; + +/** \brief Structure representing the GPS Configuration register. */ +typedef struct +{ + /** \brief The Mode field. */ + uint8_t mode; + + /** \brief The PpsSource field. */ + uint8_t ppsSource; + +} GpsConfigurationRegister; + +/** \brief Structure representing the GPS Solution - LLA register. */ +typedef struct +{ + /** \brief The Time field. */ + double time; + + /** \brief The Week field. */ + uint16_t week; + + /** \brief The GpsFix field. */ + uint8_t gpsFix; + + /** \brief The NumSats field. */ + uint8_t numSats; + + /** \brief The Lla field. */ + vec3d lla; + + /** \brief The NedVel field. */ + vec3f nedVel; + + /** \brief The NedAcc field. */ + vec3f nedAcc; + + /** \brief The SpeedAcc field. */ + float speedAcc; + + /** \brief The TimeAcc field. */ + float timeAcc; + +} GpsSolutionLlaRegister; + +/** \brief Structure representing the GPS Solution - ECEF register. */ +typedef struct +{ + /** \brief The Tow field. */ + double tow; + + /** \brief The Week field. */ + uint16_t week; + + /** \brief The GpsFix field. */ + uint8_t gpsFix; + + /** \brief The NumSats field. */ + uint8_t numSats; + + /** \brief The Position field. */ + vec3d position; + + /** \brief The Velocity field. */ + vec3f velocity; + + /** \brief The PosAcc field. */ + vec3f posAcc; + + /** \brief The SpeedAcc field. */ + float speedAcc; + + /** \brief The TimeAcc field. */ + float timeAcc; + +} GpsSolutionEcefRegister; + +/** \brief Structure representing the INS Solution - LLA register. */ +typedef struct +{ + /** \brief The Time field. */ + double time; + + /** \brief The Week field. */ + uint16_t week; + + /** \brief The Status field. */ + uint16_t status; + + /** \brief The YawPitchRoll field. */ + vec3f yawPitchRoll; + + /** \brief The Position field. */ + vec3d position; + + /** \brief The NedVel field. */ + vec3f nedVel; + + /** \brief The AttUncertainty field. */ + float attUncertainty; + + /** \brief The PosUncertainty field. */ + float posUncertainty; + + /** \brief The VelUncertainty field. */ + float velUncertainty; + +} InsSolutionLlaRegister; + +/** \brief Structure representing the INS Solution - ECEF register. */ +typedef struct +{ + /** \brief The Time field. */ + double time; + + /** \brief The Week field. */ + uint16_t week; + + /** \brief The Status field. */ + uint16_t status; + + /** \brief The YawPitchRoll field. */ + vec3f yawPitchRoll; + + /** \brief The Position field. */ + vec3d position; + + /** \brief The Velocity field. */ + vec3f velocity; + + /** \brief The AttUncertainty field. */ + float attUncertainty; + + /** \brief The PosUncertainty field. */ + float posUncertainty; + + /** \brief The VelUncertainty field. */ + float velUncertainty; + +} InsSolutionEcefRegister; + +/** \brief Structure representing the INS Basic Configuration register for a VN-200 sensor. */ +typedef struct +{ + /** \brief The Scenario field. */ + uint8_t scenario; + + /** \brief The AhrsAiding field. */ + uint8_t ahrsAiding; + +} InsBasicConfigurationRegisterVn200; + +/** \brief Structure representing the INS Basic Configuration register for a VN-300 sensor. */ +typedef struct +{ + /** \brief The Scenario field. */ + uint8_t scenario; + + /** \brief The AhrsAiding field. */ + uint8_t ahrsAiding; + + /** \brief The EstBaseline field. */ + uint8_t estBaseline; + +} InsBasicConfigurationRegisterVn300; + +/** \brief Structure representing the INS Advanced Configuration register. */ +typedef struct +{ + /** \brief The UseMag field. */ + uint8_t useMag; + + /** \brief The UsePres field. */ + uint8_t usePres; + + /** \brief The PosAtt field. */ + uint8_t posAtt; + + /** \brief The VelAtt field. */ + uint8_t velAtt; + + /** \brief The VelBias field. */ + uint8_t velBias; + + /** \brief The UseFoam field. */ + uint8_t useFoam; + + /** \brief The GPSCovType field. */ + uint8_t gpsCovType; + + /** \brief The VelCount field. */ + uint8_t velCount; + + /** \brief The VelInit field. */ + float velInit; + + /** \brief The MoveOrigin field. */ + float moveOrigin; + + /** \brief The GPSTimeout field. */ + float gpsTimeout; + + /** \brief The DeltaLimitPos field. */ + float deltaLimitPos; + + /** \brief The DeltaLimitVel field. */ + float deltaLimitVel; + + /** \brief The MinPosUncertainty field. */ + float minPosUncertainty; + + /** \brief The MinVelUncertainty field. */ + float minVelUncertainty; + +} InsAdvancedConfigurationRegister; + +/** \brief Structure representing the INS State - LLA register. */ +typedef struct +{ + /** \brief The YawPitchRoll field. */ + vec3f yawPitchRoll; + + /** \brief The Position field. */ + vec3d position; + + /** \brief The Velocity field. */ + vec3f velocity; + + /** \brief The Accel field. */ + vec3f accel; + + /** \brief The AngularRate field. */ + vec3f angularRate; + +} InsStateLlaRegister; + +/** \brief Structure representing the INS State - ECEF register. */ +typedef struct +{ + /** \brief The YawPitchRoll field. */ + vec3f yawPitchRoll; + + /** \brief The Position field. */ + vec3d position; + + /** \brief The Velocity field. */ + vec3f velocity; + + /** \brief The Accel field. */ + vec3f accel; + + /** \brief The AngularRate field. */ + vec3f angularRate; + +} InsStateEcefRegister; + +/** \brief Structure representing the Startup Filter Bias Estimate register. */ +typedef struct +{ + /** \brief The GyroBias field. */ + vec3f gyroBias; + + /** \brief The AccelBias field. */ + vec3f accelBias; + + /** \brief The PressureBias field. */ + float pressureBias; + +} StartupFilterBiasEstimateRegister; + +/** \brief Structure representing the Delta Theta and Delta Velocity register. */ +typedef struct +{ + /** \brief The DeltaTime field. */ + float deltaTime; + + /** \brief The DeltaTheta field. */ + vec3f deltaTheta; + + /** \brief The DeltaVelocity field. */ + vec3f deltaVelocity; + +} DeltaThetaAndDeltaVelocityRegister; + +/** \brief Structure representing the Delta Theta and Delta Velocity Configuration register. */ +typedef struct +{ + /** \brief The IntegrationFrame field. */ + uint8_t integrationFrame; + + /** \brief The GyroCompensation field. */ + uint8_t gyroCompensation; + + /** \brief The AccelCompensation field. */ + uint8_t accelCompensation; + +} DeltaThetaAndDeltaVelocityConfigurationRegister; + +/** \brief Structure representing the Reference Vector Configuration register. */ +typedef struct +{ + /** \brief The UseMagModel field. */ + uint8_t useMagModel; + + /** \brief The UseGravityModel field. */ + uint8_t useGravityModel; + + /** \brief The RecalcThreshold field. */ + uint32_t recalcThreshold; + + /** \brief The Year field. */ + float year; + + /** \brief The Position field. */ + vec3d position; + +} ReferenceVectorConfigurationRegister; + +/** \brief Structure representing the Gyro Compensation register. */ +typedef struct +{ + /** \brief The C field. */ + mat3f c; + + /** \brief The B field. */ + vec3f b; + +} GyroCompensationRegister; + +/** \brief Structure representing the IMU Filtering Configuration register. */ +typedef struct +{ + /** \brief The MagWindowSize field. */ + uint16_t magWindowSize; + + /** \brief The AccelWindowSize field. */ + uint16_t accelWindowSize; + + /** \brief The GyroWindowSize field. */ + uint16_t gyroWindowSize; + + /** \brief The TempWindowSize field. */ + uint16_t tempWindowSize; + + /** \brief The PresWindowSize field. */ + uint16_t presWindowSize; + + /** \brief The MagFilterMode field. */ + uint8_t magFilterMode; + + /** \brief The AccelFilterMode field. */ + uint8_t accelFilterMode; + + /** \brief The GyroFilterMode field. */ + uint8_t gyroFilterMode; + + /** \brief The TempFilterMode field. */ + uint8_t tempFilterMode; + + /** \brief The PresFilterMode field. */ + uint8_t presFilterMode; + +} ImuFilteringConfigurationRegister; + +/** \brief Structure representing the GPS Compass Baseline register. */ +typedef struct +{ + /** \brief The Position field. */ + vec3f position; + + /** \brief The Uncertainty field. */ + vec3f uncertainty; + +} GpsCompassBaselineRegister; + +/** \brief Structure representing the GPS Compass Estimated Baseline register. */ +typedef struct +{ + /** \brief The EstBaselineUsed field. */ + uint8_t estBaselineUsed; + + /** \brief The NumMeas field. */ + uint16_t numMeas; + + /** \brief The Position field. */ + vec3f position; + + /** \brief The Uncertainty field. */ + vec3f uncertainty; + +} GpsCompassEstimatedBaselineRegister; + +/** \brief Structure representing the IMU Rate Configuration register. */ +typedef struct +{ + /** \brief The imuRate field. */ + uint16_t imuRate; + + /** \brief The NavDivisor field. */ + uint16_t navDivisor; + + /** \brief The filterTargetRate field. */ + float filterTargetRate; + + /** \brief The filterMinRate field. */ + float filterMinRate; + +} ImuRateConfigurationRegister; + +/** \brief Structure representing the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register. */ +typedef struct +{ + /** \brief The YawPitchRoll field. */ + vec3f yawPitchRoll; + + /** \brief The BodyAccel field. */ + vec3f bodyAccel; + + /** \brief The Gyro field. */ + vec3f gyro; + +} YawPitchRollTrueBodyAccelerationAndAngularRatesRegister; + +/** \brief Structure representing the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register. */ +typedef struct +{ + /** \brief The YawPitchRoll field. */ + vec3f yawPitchRoll; + + /** \brief The InertialAccel field. */ + vec3f inertialAccel; + + /** \brief The Gyro field. */ + vec3f gyro; + +} YawPitchRollTrueInertialAccelerationAndAngularRatesRegister; + +/* \} */ + +typedef void (*VnSensor_PacketFoundHandler)(void *userData, VnUartPacket *packet, size_t runningIndex); + +/** \brief Helpful structure for working with VectorNav sensors. */ +typedef struct +{ + VnSerialPort serialPort; + + /* Error detection mode to use for outgoing packets. */ + VnErrorDetectionMode sendErrorDetectionMode; + + /* Timeout duration for waiting for a response from the sensor. */ + uint16_t responseTimeoutMs; + + /* Delay between retransmitting commands. */ + uint16_t retransmitDelayMs; + + VnCriticalSection transactionCS; + + VnEvent newResponsesEvent; + + /* Indicates if the transaction function is waiting for a response. */ + bool waitingForResponse; + + /* Indicates if bootloader responses are expected */ + bool bootloaderFilter; + + /* Indicates if a response is waiting for processing by the transaction functions. */ + bool responseWaitingForProcessing; + + size_t runningDataIndex; + + VnUartPacketFinder packetFinder; + + VnSensor_PacketFoundHandler asyncPacketFoundHandler; + void *asyncPacketFoundHandlerUserData; + + VnSensor_PacketFoundHandler errorMessageReceivedHandler; + void *errorMessageReceivedHandlerUserData; + + size_t responseLength; + + /* Holds any received response from the sensor for processing in our transaction functions. */ + char response[0x100]; + +} VnSensor; + +/** \brief Initializes a VnSensor structure. +* +* \param[in] sensor The structure to initialize. +* \return Any errors encountered. */ +VnError VnSensor_initialize(VnSensor* sensor); + +/** \brief Connects to a VectorNav sensor. + * + * \param[in] sensor The VnSensor 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 VnSensor_connect(VnSensor* sensor, const char* portName, uint32_t baudrate); + +/** \brief Disconnects from a VectorNav sensor. +* +* \param[in] sensor The associated sensor. +* \return Any errors encountered. */ +VnError VnSensor_disconnect(VnSensor* sensor); + +/** \brief Issues a change baudrate to the VectorNav sensor and then reconnectes +* the attached serial port at the new baudrate. +* +* \param[in] sensor The VnSensor structure. +* \param[in] baudrate The new sensor baudrate. +* \return Any errors encountered. */ +VnError VnSensor_changeBaudrate(VnSensor* sensor, uint32_t baudrate); + +/** \brief Sends the provided command and returns the response from the sensor. +* +* If the command does not have an asterisk '*', the a checksum will be performed +* and appended based on the current error detection mode. Also, if the line-ending +* \\r\\n is not present, these will be added also. +* +* \param[in] sensor The associated VnSensor. +* \param[in] toSend The command to send to the sensor. +* \param[in] toSendLength The number of bytes provided in the toSend buffer. +* \param[out] response The response received from the sensor. +* \param[in,out] responseLength The size of the provided response buffer and will be +* set with the returned response length. +* \return Any errors encountered. */ +VnError VnSensor_transaction(VnSensor* sensor, char* toSend, size_t toSendLength, char* response, size_t* responseLength); + +/** \brief Indicates if the VnSensor is connected. +* +* \param[in] sensor The associated VnSensor. +* \return true if the VnSensor is connected; otherwise false. */ +bool VnSensor_isConnected(VnSensor* sensor); + +/** \brief Issues a Write Settings command to the VectorNav Sensor. +* +* \param[in] sensor The associated VnSensor. +* \param[in] waitForReply Indicates if the method should wait for a response +* from the sensor. +* \return Any errors encountered*/ +VnError VnSensor_writeSettings(VnSensor* sensor, bool waitForReply); + +/** \brief Issues a Restore Factory Settings command to the VectorNav sensor. +* +* \param[in] sensor The associated VnSensor. +* \param[in] waitForReply Indicates if the method should wait for a response +* from the sensor. +* \return Any errors encountered*/ +VnError VnSensor_restoreFactorySettings(VnSensor* sensor, bool waitForReply); + +/** \brief Issues a Reset command to the VectorNav sensor. +* +* \param[in] sensor The associated VnSensor. +* \param[in] waitForReply Indicates if the method should wait for a response +* from the sensor. +* \return Any errors encountered*/ +VnError VnSensor_reset(VnSensor* sensor, bool waitForReply); + +/** \brief Issues a firmware update command to the VectorNav sensor. +* +* \param[in] sensor The associated VnSensor. +* \param[in] waitForReply Indicates if the method should wait for a response +* from the sensor. +* \return Any errors encountered*/ +VnError VnSensor_firmwareUpdateMode(VnSensor* sensor, bool waitForReply); + +/** \brief Issues a tare command to the VectorNav Sensor. +* +* \param[in] sensor The associated VnSensor. +* \param[in] waitForReply Indicates if the method should wait for a response +* from the sensor. +* \return Any errors encountered*/ +VnError VnSensor_tare(VnSensor* sensor, bool waitForReply); + +/** \brief Issues a command to the VectorNav Sensor to set the Gyro's bias. +* +* \param[in] sensor The associated VnSensor. +* \param[in] waitForReply Indicates if the method should wait for a response +* from the sensor. +* \return Any errors encountered*/ +VnError VnSensor_setGyroBias(VnSensor* sensor, bool waitForReply); + +/** \brief Command to inform the VectorNav Sensor if there is a magnetic disturbance present. +* +* \param[in] sensor The associated VnSensor. +* \param[in] disturbancePresent Indicates the presense of a disturbance. +* \param[in] waitForReply Indicates if the method should wait for a response +* from the sensor. +* \return Any errors encountered*/ +VnError VnSensor_magneticDisturbancePresent(VnSensor* sensor, bool disturbancePresent, bool waitForReply); + +/** \brief Command to inform the VectorNav Sensor if there is an acceleration disturbance present. +* +* \param[in] sensor The associated VnSensor. +* \param[in] disturbancePresent Indicates the presense of a disturbance. +* \param[in] waitForReply Indicates if the method should wait for a response +* from the sensor. +* \return Any errors encountered*/ +VnError VnSensor_accelerationDisturbancePresent(VnSensor* sensor, bool disturbancePresent, bool waitForReply); + +/** \brief Checks if we are able to send and receive communication with a sensor. + * + * \param[in] sensor The associated sensor. + * \return true if we can communicate with a sensor; otherwise false. */ +bool VnSensor_verifySensorConnectivity(VnSensor* sensor); + +/** \brief Returns the current response timeout value in milliseconds used for + * communication with a sensor. + * + * The response timeout is used on commands that require a response to be + * received from the sensor. If a response has not been received from the sensor + * in the amount of time specified by this value, the called function will + * return an E_TIMEOUT error. + * + * \param[in] sensor The associated VnSensor. + * \return The current response timeout value in milliseconds. */ +uint16_t VnSensor_getResponseTimeoutMs(VnSensor* sensor); + +/** \brief Sets the current response timeout value in milliseconds used for + * communication with a sensor. + * + * The response timeout is used on commands that require a response to be + * received from the sensor. If a response has not been received from the sensor + * in the amount of time specified by this value, the called function will + * return an E_TIMEOUT error. + * + * \param[in] sensor The associated VnSensor. + * \param[in] responseTimeoutMs The new value for the response timeout in milliseconds. + * \return Any errors encountered. */ +VnError VnSensor_setResponseTimeoutMs(VnSensor* sensor, uint16_t reponseTimeoutMs); + +/** \brief Gets the current retransmit delay used for communication with a + * sensor. + * + * During the time that the VnSensor is awaiting a response from a sensor, the + * command will be retransmitted to the sensor at the interval specified by this + * value. + * + * \param[in] sensor The associated VnSensor. + * \return The current retransmit delay value in milliseconds. */ +uint16_t VnSensor_getRetransmitDelayMs(VnSensor* sensor); + +/** \brief Sets the current retransmit delay used for communication with a + * sensor. + * + * During the time that the VnSensor is awaiting a response from a sensor, the + * command will be retransmitted to the sensor at the interval specified by this + * value. + * + * \param[in] sensor The associated VnSensor. + * \param[in] retransmitDelayMs The new value for the retransmit delay in milliseconds. + * \return Any errors encountered. */ +VnError VnSensor_setRetransmitDelayMs(VnSensor* sensor, uint16_t retransmitDelayMs); + +/** \brief Allows registering a callback for notification of when an asynchronous data packet is received. + * + * \param[in] sensor The associated VnSensor. + * \param[in] handler The callback handler. + * \param[in] userData Data which will be provided with all callbacks. + * \return Any errors encountered. */ +VnError VnSensor_registerAsyncPacketReceivedHandler(VnSensor *sensor, VnSensor_PacketFoundHandler handler, void *userData); + +/** \brief Allows unregistering from callback notifications when asynchronous data packets are received. + * + * \param[in] sensor The associated sensor. */ +VnError VnSensor_unregisterAsyncPacketReceivedHandler(VnSensor *sensor); + +/** \brief Allows registering a callback for notification of when a sensor error message is received. + * + * \param[in] sensor The associated VnSensor. + * \param[in] handler The callback handler. + * \param[in] userData Data which will be provided with all callbacks. + * \return Any errors encountered. */ +VnError VnSensor_registerErrorPacketReceivedHandler(VnSensor *sensor, VnSensor_PacketFoundHandler handler, void *userData); + +/** \brief Allows unregistering callbacks for notifications of when a sensor error message is recieved. + * + * \param[in] sensor The associated VnSensor. + * \return Any errors encountered. */ +VnError VnSensor_unregisterErrorPacketReceivedHandler(VnSensor *sensor); + +/** \defgroup registerAccessMethods Register Access Methods + * \brief This group of methods provide access to read and write to the + * sensor's registers. + * + * \{ */ + +/** \brief Reads the Binary Output 1 register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readBinaryOutput1(VnSensor *sensor, BinaryOutputRegister *fields); + +/** \brief Writes to the Binary Output 1 register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The register's fields. + * \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + * \return Any errors encountered. */ +VnError VnSensor_writeBinaryOutput1(VnSensor *sensor, BinaryOutputRegister *fields, bool waitForReply); + +/** \brief Reads the Binary Output 2 register. +* +* \param[in] sensor The associated VnSensor. +* \param[in] fields The register's values. +* \return Any errors encountered. */ +VnError VnSensor_readBinaryOutput2(VnSensor *sensor, BinaryOutputRegister *fields); + +/** \brief Writes to the Binary Output 2 register. +* +* \param[in] sensor The associated VnSensor. +* \param[in] fields The register's fields. +* \param[in] waitForReply Indicates if the method should wait for a response from the sensor. +* \return Any errors encountered. */ +VnError VnSensor_writeBinaryOutput2(VnSensor *sensor, BinaryOutputRegister *fields, bool waitForReply); + +/** \brief Reads the Binary Output 3 register. +* +* \param[in] sensor The associated VnSensor. +* \param[in] fields The register's values. +* \return Any errors encountered. */ +VnError VnSensor_readBinaryOutput3(VnSensor *sensor, BinaryOutputRegister *fields); + +/** \brief Writes to the Binary Output 3 register. +* +* \param[in] sensor The associated VnSensor. +* \param[in] fields The register's fields. +* \param[in] waitForReply Indicates if the method should wait for a response from the sensor. +* \return Any errors encountered. */ +VnError VnSensor_writeBinaryOutput3(VnSensor *sensor, BinaryOutputRegister *fields, bool waitForReply); + +#ifdef EXTRA + +/** \brief Reads the Binary Output 4 register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readBinaryOutput4(VnSensor *sensor, BinaryOutputRegister *fields); + +/** \brief Writes to the Binary Output 4 register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The register's fields. + * \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + * \return Any errors encountered. */ +VnError VnSensor_writeBinaryOutput4(VnSensor *sensor, BinaryOutputRegister *fields, bool waitForReply); + +/** \brief Reads the Binary Output 5 register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readBinaryOutput5(VnSensor *sensor, BinaryOutputRegister *fields); + +/** \brief Writes to the Binary Output 5 register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The register's fields. + * \param[in] waitForReply Indicates if the method should wait for a response from the sensor. + * \return Any errors encountered. */ +VnError VnSensor_writeBinaryOutput5(VnSensor *sensor, BinaryOutputRegister *fields, bool waitForReply); + +#endif + +/** \brief Reads the User Tag register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] tagBuffer Buffer to place the read register value. + * \param[in] tagBufferLength Length of the provided buffer. + * \return Any errors encountered. */ +VnError VnSensor_readUserTag(VnSensor *sensor, char *tagBuffer, size_t tagBufferLength); + +/** \brief Writes to the User Tag register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] tag The value to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeUserTag(VnSensor *sensor, char* tag, bool waitForReply); + +/** \brief Reads the Model Number register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] productNameBuffer Buffer to place the read register value. + * \param[in] productNameBufferLength Length of the provided buffer. + * \return Any errors encountered. */ +VnError VnSensor_readModelNumber(VnSensor *sensor, char *productNameBuffer, size_t productNameBufferLength); + +/** \brief Reads the Hardware Revision register. + * + * \param[in] sensor The associated VnSensor. + * \return Any errors encountered. */ +VnError VnSensor_readHardwareRevision(VnSensor *sensor, uint32_t *revision); + +/** \brief Reads the Serial Number register. + * + * \param[in] sensor The associated VnSensor. + * \return Any errors encountered. */ +VnError VnSensor_readSerialNumber(VnSensor *sensor, uint32_t *serialNum); + +/** \brief Reads the Firmware Version register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] firmwareVersionBuffer Buffer to place the read register value. + * \param[in] firmwareVersionBufferLength Length of the provided buffer. + * \return Any errors encountered. */ +VnError VnSensor_readFirmwareVersion(VnSensor *sensor, char *firmwareVersionBuffer, size_t firmwareVersionBufferLength); + +/** \brief Reads the Serial Baud Rate register. + * + * \param[in] sensor The associated VnSensor. + * \return Any errors encountered. */ +VnError VnSensor_readSerialBaudRate(VnSensor *sensor, uint32_t *baudrate); + +/** \brief Writes to the Serial Baud Rate register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] baudrate The value to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeSerialBaudRate(VnSensor *sensor, uint32_t baudrate, bool waitForReply); + +/** \brief Reads the Async Data Output Type register. + * + * \param[in] sensor The associated VnSensor. + * \return Any errors encountered. */ +VnError VnSensor_readAsyncDataOutputType(VnSensor *sensor, VnAsciiAsync *ador); + +/** \brief Writes to the Async Data Output Type register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] ador The value to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeAsyncDataOutputType(VnSensor *sensor, VnAsciiAsync ador, bool waitForReply); + +/** \brief Reads the Async Data Output Frequency register. + * + * \param[in] sensor The associated VnSensor. + * \return Any errors encountered. */ +VnError VnSensor_readAsyncDataOutputFrequency(VnSensor *sensor, uint32_t *adof); + +/** \brief Writes to the Async Data Output Frequency register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] adof The value to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeAsyncDataOutputFrequency(VnSensor *sensor, uint32_t adof, bool waitForReply); + +/** \brief Reads the Yaw Pitch Roll register. + * + * \param[in] sensor The associated VnSensor. + * \return Any errors encountered. */ +VnError VnSensor_readYawPitchRoll(VnSensor *sensor, vec3f *yawPitchRoll); + +/** \brief Reads the Attitude Quaternion register. + * + * \param[in] sensor The associated VnSensor. + * \return Any errors encountered. */ +VnError VnSensor_readAttitudeQuaternion(VnSensor *sensor, vec4f *quat); + +/** \brief Reads the Quaternion, Magnetic, Acceleration and Angular Rates register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readQuaternionMagneticAccelerationAndAngularRates(VnSensor *sensor, QuaternionMagneticAccelerationAndAngularRatesRegister *fields); + +/** \brief Reads the Magnetic Measurements register. + * + * \param[in] sensor The associated VnSensor. + * \return Any errors encountered. */ +VnError VnSensor_readMagneticMeasurements(VnSensor *sensor, vec3f *mag); + +/** \brief Reads the Acceleration Measurements register. + * + * \param[in] sensor The associated VnSensor. + * \return Any errors encountered. */ +VnError VnSensor_readAccelerationMeasurements(VnSensor *sensor, vec3f *accel); + +/** \brief Reads the Angular Rate Measurements register. + * + * \param[in] sensor The associated VnSensor. + * \return Any errors encountered. */ +VnError VnSensor_readAngularRateMeasurements(VnSensor *sensor, vec3f *gyro); + +/** \brief Reads the Magnetic, Acceleration and Angular Rates register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readMagneticAccelerationAndAngularRates(VnSensor *sensor, MagneticAccelerationAndAngularRatesRegister *fields); + +/** \brief Reads the Magnetic and Gravity Reference Vectors register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readMagneticAndGravityReferenceVectors(VnSensor *sensor, MagneticAndGravityReferenceVectorsRegister *fields); + +/** \brief Writes to the Magnetic and Gravity Reference Vectors register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The values to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeMagneticAndGravityReferenceVectors(VnSensor *sensor, MagneticAndGravityReferenceVectorsRegister fields, bool waitForReply); + +/** \brief Reads the Magnetometer Compensation register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readMagnetometerCompensation(VnSensor *sensor, MagnetometerCompensationRegister *fields); + +/** \brief Writes to the Magnetometer Compensation register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The values to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeMagnetometerCompensation(VnSensor *sensor, MagnetometerCompensationRegister fields, bool waitForReply); + +/** \brief Reads the Acceleration Compensation register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readAccelerationCompensation(VnSensor *sensor, AccelerationCompensationRegister *fields); + +/** \brief Writes to the Acceleration Compensation register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The values to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeAccelerationCompensation(VnSensor *sensor, AccelerationCompensationRegister fields, bool waitForReply); + +/** \brief Reads the Reference Frame Rotation register. + * + * \param[in] sensor The associated VnSensor. + * \return Any errors encountered. */ +VnError VnSensor_readReferenceFrameRotation(VnSensor *sensor, mat3f *c); + +/** \brief Writes to the Reference Frame Rotation register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] c The value to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeReferenceFrameRotation(VnSensor *sensor, mat3f c, bool waitForReply); + +/** \brief Reads the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readYawPitchRollMagneticAccelerationAndAngularRates(VnSensor *sensor, YawPitchRollMagneticAccelerationAndAngularRatesRegister *fields); + +/** \brief Reads the Communication Protocol Control register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readCommunicationProtocolControl(VnSensor *sensor, CommunicationProtocolControlRegister *fields); + +/** \brief Writes to the Communication Protocol Control register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The values to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeCommunicationProtocolControl(VnSensor *sensor, CommunicationProtocolControlRegister fields, bool waitForReply); + +/** \brief Reads the Synchronization Control register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readSynchronizationControl(VnSensor *sensor, SynchronizationControlRegister *fields); + +/** \brief Writes to the Synchronization Control register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The values to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeSynchronizationControl(VnSensor *sensor, SynchronizationControlRegister fields, bool waitForReply); + +/** \brief Reads the Synchronization Status register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readSynchronizationStatus(VnSensor *sensor, SynchronizationStatusRegister *fields); + +/** \brief Writes to the Synchronization Status register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The values to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeSynchronizationStatus(VnSensor *sensor, SynchronizationStatusRegister fields, bool waitForReply); + +/** \brief Reads the VPE Basic Control register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readVpeBasicControl(VnSensor *sensor, VpeBasicControlRegister *fields); + +/** \brief Writes to the VPE Basic Control register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The values to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeVpeBasicControl(VnSensor *sensor, VpeBasicControlRegister fields, bool waitForReply); + +/** \brief Reads the VPE Magnetometer Basic Tuning register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readVpeMagnetometerBasicTuning(VnSensor *sensor, VpeMagnetometerBasicTuningRegister *fields); + +/** \brief Writes to the VPE Magnetometer Basic Tuning register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The values to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeVpeMagnetometerBasicTuning(VnSensor *sensor, VpeMagnetometerBasicTuningRegister fields, bool waitForReply); + +/** \brief Reads the VPE Accelerometer Basic Tuning register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readVpeAccelerometerBasicTuning(VnSensor *sensor, VpeAccelerometerBasicTuningRegister *fields); + +/** \brief Writes to the VPE Accelerometer Basic Tuning register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The values to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeVpeAccelerometerBasicTuning(VnSensor *sensor, VpeAccelerometerBasicTuningRegister fields, bool waitForReply); + +/** \brief Reads the Magnetometer Calibration Control register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readMagnetometerCalibrationControl(VnSensor *sensor, MagnetometerCalibrationControlRegister *fields); + +/** \brief Writes to the Magnetometer Calibration Control register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The values to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeMagnetometerCalibrationControl(VnSensor *sensor, MagnetometerCalibrationControlRegister fields, bool waitForReply); + +/** \brief Reads the Calculated Magnetometer Calibration register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readCalculatedMagnetometerCalibration(VnSensor *sensor, CalculatedMagnetometerCalibrationRegister *fields); + +/** \brief Reads the Velocity Compensation Measurement register. + * + * \param[in] sensor The associated VnSensor. + * \return Any errors encountered. */ +VnError VnSensor_readVelocityCompensationMeasurement(VnSensor *sensor, vec3f *velocity); + +/** \brief Writes to the Velocity Compensation Measurement register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] velocity The value to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeVelocityCompensationMeasurement(VnSensor *sensor, vec3f velocity, bool waitForReply); + +/** \brief Reads the Velocity Compensation Control register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readVelocityCompensationControl(VnSensor *sensor, VelocityCompensationControlRegister *fields); + +/** \brief Writes to the Velocity Compensation Control register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The values to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeVelocityCompensationControl(VnSensor *sensor, VelocityCompensationControlRegister fields, bool waitForReply); + +/** \brief Reads the IMU Measurements register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readImuMeasurements(VnSensor *sensor, ImuMeasurementsRegister *fields); + +/** \brief Reads the GPS Configuration register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readGpsConfiguration(VnSensor *sensor, GpsConfigurationRegister *fields); + +/** \brief Writes to the GPS Configuration register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The values to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeGpsConfiguration(VnSensor *sensor, GpsConfigurationRegister fields, bool waitForReply); + +/** \brief Reads the GPS Antenna Offset register. + * + * \param[in] sensor The associated VnSensor. + * \return Any errors encountered. */ +VnError VnSensor_readGpsAntennaOffset(VnSensor *sensor, vec3f *position); + +/** \brief Writes to the GPS Antenna Offset register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] position The value to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeGpsAntennaOffset(VnSensor *sensor, vec3f position, bool waitForReply); + +/** \brief Reads the GPS Solution - LLA register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readGpsSolutionLla(VnSensor *sensor, GpsSolutionLlaRegister *fields); + +/** \brief Reads the GPS Solution - ECEF register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readGpsSolutionEcef(VnSensor *sensor, GpsSolutionEcefRegister *fields); + +/** \brief Reads the INS Solution - LLA register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readInsSolutionLla(VnSensor *sensor, InsSolutionLlaRegister *fields); + +/** \brief Reads the INS Solution - ECEF register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readInsSolutionEcef(VnSensor *sensor, InsSolutionEcefRegister *fields); + +/** \brief Reads the INS Basic Configuration register for a VN-200 sensor. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readInsBasicConfigurationVn200(VnSensor *sensor, InsBasicConfigurationRegisterVn200 *fields); + +/** \brief Writes to the INS Basic Configuration register for a VN-200 sensor. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The values to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeInsBasicConfigurationVn200(VnSensor *sensor, InsBasicConfigurationRegisterVn200 fields, bool waitForReply); + +/** \brief Reads the INS Basic Configuration register for a VN-300 sensor. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readInsBasicConfigurationVn300(VnSensor *sensor, InsBasicConfigurationRegisterVn300 *fields); + +/** \brief Writes to the INS Basic Configuration register for a VN-300 sensor. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The values to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeInsBasicConfigurationVn300(VnSensor *sensor, InsBasicConfigurationRegisterVn300 fields, bool waitForReply); + +/** \brief Reads the INS State - LLA register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readInsStateLla(VnSensor *sensor, InsStateLlaRegister *fields); + +/** \brief Reads the INS State - ECEF register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readInsStateEcef(VnSensor *sensor, InsStateEcefRegister *fields); + +/** \brief Reads the Startup Filter Bias Estimate register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readStartupFilterBiasEstimate(VnSensor *sensor, StartupFilterBiasEstimateRegister *fields); + +/** \brief Writes to the Startup Filter Bias Estimate register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The values to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeStartupFilterBiasEstimate(VnSensor *sensor, StartupFilterBiasEstimateRegister fields, bool waitForReply); + +/** \brief Reads the Delta Theta and Delta Velocity register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readDeltaThetaAndDeltaVelocity(VnSensor *sensor, DeltaThetaAndDeltaVelocityRegister *fields); + +/** \brief Reads the Delta Theta and Delta Velocity Configuration register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readDeltaThetaAndDeltaVelocityConfiguration(VnSensor *sensor, DeltaThetaAndDeltaVelocityConfigurationRegister *fields); + +/** \brief Writes to the Delta Theta and Delta Velocity Configuration register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The values to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeDeltaThetaAndDeltaVelocityConfiguration(VnSensor *sensor, DeltaThetaAndDeltaVelocityConfigurationRegister fields, bool waitForReply); + +/** \brief Reads the Reference Vector Configuration register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readReferenceVectorConfiguration(VnSensor *sensor, ReferenceVectorConfigurationRegister *fields); + +/** \brief Writes to the Reference Vector Configuration register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The values to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeReferenceVectorConfiguration(VnSensor *sensor, ReferenceVectorConfigurationRegister fields, bool waitForReply); + +/** \brief Reads the Gyro Compensation register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readGyroCompensation(VnSensor *sensor, GyroCompensationRegister *fields); + +/** \brief Writes to the Gyro Compensation register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The values to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeGyroCompensation(VnSensor *sensor, GyroCompensationRegister fields, bool waitForReply); + +/** \brief Reads the IMU Filtering Configuration register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readImuFilteringConfiguration(VnSensor *sensor, ImuFilteringConfigurationRegister *fields); + +/** \brief Writes to the IMU Filtering Configuration register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The values to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeImuFilteringConfiguration(VnSensor *sensor, ImuFilteringConfigurationRegister fields, bool waitForReply); + +/** \brief Reads the GPS Compass Baseline register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readGpsCompassBaseline(VnSensor *sensor, GpsCompassBaselineRegister *fields); + +/** \brief Writes to the GPS Compass Baseline register. + * + * \param[in] sensor The associated VnSensor. + * \param[in] fields The values to write to the register. + * \param[in] waitForReply Set to true to wait for a response from the sensor; otherwise set to false to just immediately send the command and return. + * \return Any errors encountered. */ +VnError VnSensor_writeGpsCompassBaseline(VnSensor *sensor, GpsCompassBaselineRegister fields, bool waitForReply); + +/** \brief Reads the GPS Compass Estimated Baseline register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readGpsCompassEstimatedBaseline(VnSensor *sensor, GpsCompassEstimatedBaselineRegister *fields); + +/** \brief Reads the Yaw, Pitch, Roll, True Body Acceleration and Angular Rates register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readYawPitchRollTrueBodyAccelerationAndAngularRates(VnSensor *sensor, YawPitchRollTrueBodyAccelerationAndAngularRatesRegister *fields); + +/** \brief Reads the Yaw, Pitch, Roll, True Inertial Acceleration and Angular Rates register. + * + * \param[in] sensor The associated VnSensor. + * \param[out] fields The register's values. + * \return Any errors encountered. */ +VnError VnSensor_readYawPitchRollTrueInertialAccelerationAndAngularRates(VnSensor *sensor, YawPitchRollTrueInertialAccelerationAndAngularRatesRegister *fields); + +/** \brief Upgrade the connected sensor with the supplied firmware file + * + * \param[in] sensor The associated VnSensor. + * \param[in] processor The target processor to switch to. + * \param[in] model The model of sensor. + * \param[in] firmware The current firmware version of sensor. + * \return Any errors encountered. */ +VnError VnSensor_switchProcessors(VnSensor* s, VnProcessorType processor, char* model, char* firmware); + +/** \brief Update the connected sensor with the supplied firmware file + * + * \param[in] sensor The associated VnSensor. + * \param]in] portName The baud rate used to update the firmware. (Can be different than the current baud rate) + * \param[in] filename The path to the VNX firmware file for the sensor. + * \return Any errors encountered. */ +VnError VnSensor_firmwareUpdate(VnSensor* s, int baudRate, char* fileName); + +/** \brief Open a firmware update file to parse. + * + * \param[in] filename The path to the VNX firmware file for the sensor. + * \return A file handle. */ +FILE* VnSensor_openFirmwareUpdateFile(char* filename); + +/** \brief Parse out and copy the next record in the firmware update file. + * + * \param[in] file The file handle returned by VnSensor_openFirmwareUpdateFile. + * \param]in] record A buffer to hold the next record from the firmware update file. + * \param[in] MaxRecordSize Pass in the current size of the record buffer. + * \return True if another record was found, otherwise false. */ +bool VnSensor_getNextFirmwareUpdateRecord(FILE* file, char* record, int MaxRecordSize); + +/** \brief Close the firmware update file + * + * \param[in] file The file handle returned by VnSensor_openFirmwareUpdateFile.*/ +void VnSensor_closeFirmwareUpdateFile(FILE* file); + +/** \brief Calibrate the bootloader baudrate once in bootloader mode + * + * \param[in] sensor The associated VnSensor.*/ +void VnSensor_calibrateBootloader(VnSensor* s); + +/** \brief Write the firmware update record to the bootloader + * + * \param[in] sensor The associated VnSensor. + * \param]in] record A line from the firmware update file. + * \return Any errors encountered. */ +VnError VnSensor_writeFirmwareUpdateRecord(VnSensor* s, char* record); + +/** \} */ + +/** \brief Converts a sensor error into a string. +* +* \param[out] out The buffer to place the string in. +* \param[in] val The SensorError value to convert to string. +* \return The converted value. +*/ +void strFromSensorError(char *out, SensorError val); + +/** \brief Converts a SyncInMode into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The SyncInMode value to convert to string. + */ +void strFromSyncInMode(char *out, VnSyncInMode val); + +/** \brief Converts a SyncInEdge into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The SyncInEdge value to convert to string. + */ +void strFromSyncInEdge(char *out, VnSyncInEdge val); + +/** \brief Converts a SyncOutMode into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The SyncOutMode value to convert to string. + */ +void strFromSyncOutMode(char *out, VnSyncOutMode val); + +/** \brief Converts a SyncOutPolarity into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The SyncOutPolarity value to convert to string. + */ +void strFromSyncOutPolarity(char *out, VnSyncOutPolarity val); + +/** \brief Converts a CountMode into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The CountMode value to convert to string. + */ +void strFromCountMode(char *out, VnCountMode val); + +/** \brief Converts a StatusMode into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The StatusMode value to convert to string. + */ +void strFromStatusMode(char *out, VnStatusMode val); + +/** \brief Converts a ChecksumMode into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The ChecksumMode value to convert to string. + */ +void strFromChecksumMode(char *out, VnChecksumMode val); + +/** \brief Converts a ErrorMode into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The ErrorMode value to convert to string. + */ +void strFromErrorMode(char *out, VnErrorMode val); + +/** \brief Converts a FilterMode into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The FilterMode value to convert to string. + */ +void strFromFilterMode(char *out, VnFilterMode val); + +/** \brief Converts a IntegrationFrame into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The IntegrationFrame value to convert to string. + */ +void strFromIntegrationFrame(char *out, VnIntegrationFrame val); + +/** \brief Converts a CompensationMode into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The CompensationMode value to convert to string. + */ +void strFromCompensationMode(char *out, VnCompensationMode val); + +/** \brief Converts a GpsFix into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The GpsFix value to convert to string. + */ +void strFromGpsFix(char *out, VnGpsFix val); + +/** \brief Converts a GpsMode into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The GpsMode value to convert to string. + */ +void strFromGpsMode(char *out, VnGpsMode val); + +/** \brief Converts a PpsSource into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The PpsSource value to convert to string. + */ +void strFromPpsSource(char *out, VnPpsSource val); + +/** \brief Converts a VpeEnable into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The VpeEnable value to convert to string. + */ +void strFromVpeEnable(char *out, VnVpeEnable val); + +/** \brief Converts a HeadingMode into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The HeadingMode value to convert to string. + */ +void strFromHeadingMode(char *out, VnHeadingMode val); + +/** \brief Converts a VpeMode into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The VpeMode value to convert to string. + */ +void strFromVpeMode(char *out, VnVpeMode val); + +/** \brief Converts a Scenario into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The Scenario value to convert to string. + */ +void strFromScenario(char *out, VnScenario val); + +/** \brief Converts a HsiMode into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The HsiMode value to convert to string. + */ +void strFromHsiMode(char *out, VnHsiMode val); + +/** \brief Converts a HsiOutput into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The HsiOutput value to convert to string. + */ +void strFromHsiOutput(char *out, VnHsiOutput val); + +/** \brief Converts a VelocityCompensationMode into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The VelocityCompensationMode value to convert to string. + */ +void strFromVelocityCompensationMode(char *out, VnVelocityCompensationMode val); + +/** \brief Converts a MagneticMode into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The MagneticMode value to convert to string. + */ +void strFromMagneticMode(char *out, VnMagneticMode val); + +/** \brief Converts a ExternalSensorMode into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The ExternalSensorMode value to convert to string. + */ +void strFromExternalSensorMode(char *out, VnExternalSensorMode val); + +/** \brief Converts a FoamInit into a string. + * + * \param[out] out The buffer to place the string in. + * \param[in] val The FoamInit value to convert to string. + */ +void strFromFoamInit(char *out, VnFoamInit val); + +#ifdef __cplusplus +} +#endif + +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#endif diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/sensors/compositedata.h b/src/drivers/ins/vectornav/libvnc/include/vn/sensors/compositedata.h new file mode 100644 index 0000000000..528d43261d --- /dev/null +++ b/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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/sensors/ezasyncdata.h b/src/drivers/ins/vectornav/libvnc/include/vn/sensors/ezasyncdata.h new file mode 100644 index 0000000000..734ec0804e --- /dev/null +++ b/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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/sensors/searcher.h b/src/drivers/ins/vectornav/libvnc/include/vn/sensors/searcher.h new file mode 100644 index 0000000000..44607b4084 --- /dev/null +++ b/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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/types.h b/src/drivers/ins/vectornav/libvnc/include/vn/types.h new file mode 100644 index 0000000000..a8f286a4b1 --- /dev/null +++ b/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 + +#else + + /* Must not have C99. */ + + /** Backup definition of size_t. */ + typedef unsigned int size_t; + +#endif + +#else +extern "C" { +#endif + + +/* Adding VectorNav data types */ +typedef struct +{ + int8_t year; /** \brief Year field. */ + uint8_t month; /** \brief Month field. */ + uint8_t day; /** \brief Day field. */ + uint8_t hour; /** \brief Hour field. */ + uint8_t min; /** \brief Min field. */ + uint8_t sec; /** \brief Sec field. */ + uint16_t ms; /** \brief Ms field. */ +} TimeUtc; + +typedef struct +{ + float gDOP; /** \brief Gdop field. */ + float pDOP; /** \brief Pdop field. */ + float tDOP; /** \brief Tdop field. */ + float vDOP; /** \brief Vdop field. */ + float hDOP; /** \brief Hdop field. */ + float nDOP; /** \brief Ndop field. */ + float eDOP; /** \brief Edop field. */ +} GpsDop; + +typedef struct +{ + uint8_t timeStatus; /** \brief timeStatus field. */ + int8_t leapSeconds; /** \brief leapSeconds field. */ +} TimeInfo; + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/util.h b/src/drivers/ins/vectornav/libvnc/include/vn/util.h new file mode 100644 index 0000000000..02388f41f0 --- /dev/null +++ b/src/drivers/ins/vectornav/libvnc/include/vn/util.h @@ -0,0 +1,273 @@ +#ifndef VNUTIL_H_INCLUDED +#define VNUTIL_H_INCLUDED + +#include + +#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 float 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 double 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 vec3f 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 vec4f 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 vec3d 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 mat3f 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 GpsDop 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 TimeUtc 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 TimeInfo 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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/util/compiler.h b/src/drivers/ins/vectornav/libvnc/include/vn/util/compiler.h new file mode 100644 index 0000000000..62e58b5fd9 --- /dev/null +++ b/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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/util/export.h b/src/drivers/ins/vectornav/libvnc/include/vn/util/export.h new file mode 100644 index 0000000000..9dc46dace8 --- /dev/null +++ b/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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/util/port.h b/src/drivers/ins/vectornav/libvnc/include/vn/util/port.h new file mode 100644 index 0000000000..890d50f316 --- /dev/null +++ b/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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/vectornav.h b/src/drivers/ins/vectornav/libvnc/include/vn/vectornav.h new file mode 100644 index 0000000000..a73665cbee --- /dev/null +++ b/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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/xplat/criticalsection.h b/src/drivers/ins/vectornav/libvnc/include/vn/xplat/criticalsection.h new file mode 100644 index 0000000000..edc44b7329 --- /dev/null +++ b/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 + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif + +#endif + +#if (defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__) + #include +#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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/xplat/event.h b/src/drivers/ins/vectornav/libvnc/include/vn/xplat/event.h new file mode 100644 index 0000000000..bce2bc5f0a --- /dev/null +++ b/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 + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif + +#endif + +#if (defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__) + #include +#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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/xplat/serialport.h b/src/drivers/ins/vectornav/libvnc/include/vn/xplat/serialport.h new file mode 100644 index 0000000000..8ac083a5e5 --- /dev/null +++ b/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 + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +#endif + +#ifdef __linux__ + #include +#elif defined __APPLE__ + #include +#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 true if the serial port is open; otherwise false. */ +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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/xplat/thread.h b/src/drivers/ins/vectornav/libvnc/include/vn/xplat/thread.h new file mode 100644 index 0000000000..a4e57c61ca --- /dev/null +++ b/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 + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif + +#endif + +#if defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__ + #include +#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 diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/xplat/time.h b/src/drivers/ins/vectornav/libvnc/include/vn/xplat/time.h new file mode 100644 index 0000000000..4e5e731d21 --- /dev/null +++ b/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 + + #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 diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/conv.c b/src/drivers/ins/vectornav/libvnc/src/vn/conv.c new file mode 100644 index 0000000000..b70082b6a5 --- /dev/null +++ b/src/drivers/ins/vectornav/libvnc/src/vn/conv.c @@ -0,0 +1,81 @@ +#include "vn/conv.h" +#include +#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); +} diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/error.c b/src/drivers/ins/vectornav/libvnc/src/vn/error.c new file mode 100644 index 0000000000..14694e90f2 --- /dev/null +++ b/src/drivers/ins/vectornav/libvnc/src/vn/error.c @@ -0,0 +1,142 @@ +#include "vn/error.h" + +#include + +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 +} diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/error_detection.c b/src/drivers/ins/vectornav/libvnc/src/vn/error_detection.c new file mode 100644 index 0000000000..c2f64b13b6 --- /dev/null +++ b/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; +} diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/math/matrix.c b/src/drivers/ins/vectornav/libvnc/src/vn/math/matrix.c new file mode 100644 index 0000000000..b82ea10917 --- /dev/null +++ b/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 */ + +/*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 diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/math/vector.c b/src/drivers/ins/vectornav/libvnc/src/vn/math/vector.c new file mode 100644 index 0000000000..e1f85f10db --- /dev/null +++ b/src/drivers/ins/vectornav/libvnc/src/vn/math/vector.c @@ -0,0 +1,118 @@ +#include "vn/math/vector.h" + +#include + +vec3d create_v3d(double x, double y, double z) +{ + vec3d v; + + v.c[0] = x; + v.c[1] = y; + v.c[2] = z; + + return v; +} + +void vn_v3_init_fa(vec3f* v, const float* fa) +{ + size_t i; + + for (i = 0; i < 3; i++) + v->c[i] = fa[i]; +} + +vec3f add_v3f_v3f(vec3f lhs, vec3f rhs) +{ + vec3f r; + + r.c[0] = lhs.c[0] + rhs.c[0]; + r.c[1] = lhs.c[1] + rhs.c[1]; + r.c[2] = lhs.c[2] + rhs.c[2]; + + return r; +} + +vec3d add_v3d_v3d(vec3d lhs, vec3d rhs) +{ + vec3d r; + + r.c[0] = lhs.c[0] + rhs.c[0]; + r.c[1] = lhs.c[1] + rhs.c[1]; + r.c[2] = lhs.c[2] + rhs.c[2]; + + return r; +} + +vec4f add_v4f_v4f(vec4f lhs, vec4f rhs) +{ + vec4f r; + + r.c[0] = lhs.c[0] + rhs.c[0]; + r.c[1] = lhs.c[1] + rhs.c[1]; + r.c[2] = lhs.c[2] + rhs.c[2]; + r.c[3] = lhs.c[3] + rhs.c[3]; + + return r; +} + +vec3f sub_v3f_v3f(vec3f lhs, vec3f rhs) +{ + vec3f r; + + r.c[0] = lhs.c[0] - rhs.c[0]; + r.c[1] = lhs.c[1] - rhs.c[1]; + r.c[2] = lhs.c[2] - rhs.c[2]; + + return r; +} + +vec3d sub_v3d_v3d(vec3d lhs, vec3d rhs) +{ + vec3d r; + + r.c[0] = lhs.c[0] - rhs.c[0]; + r.c[1] = lhs.c[1] - rhs.c[1]; + r.c[2] = lhs.c[2] - rhs.c[2]; + + return r; +} + +vec4f sub_v4f_v4f(vec4f lhs, vec4f rhs) +{ + vec4f r; + + r.c[0] = lhs.c[0] - rhs.c[0]; + r.c[1] = lhs.c[1] - rhs.c[1]; + r.c[2] = lhs.c[2] - rhs.c[2]; + r.c[3] = lhs.c[3] - rhs.c[3]; + + return r; +} + +#if defined(_MSC_VER) + /* Disable warnings regarding using sprintf_s since these + * function signatures do not provide us with information + * about the length of 'out'. */ + #pragma warning(push) + #pragma warning(disable:4996) +#endif + +void str_vec3f(char* out, vec3f v) +{ + sprintf(out, "(%f; %f; %f)", v.c[0], v.c[1], v.c[2]); +} + +void str_vec3d(char* out, vec3d v) +{ + sprintf(out, "(%f; %f; %f)", v.c[0], v.c[1], v.c[2]); +} + +void str_vec4f(char* out, vec4f v) +{ + sprintf(out, "(%f; %f; %f; %f)", v.c[0], v.c[1], v.c[2], v.c[3]); +} + + +#if defined(_MSC_VER) + #pragma warning(pop) +#endif diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/protocol/spi.c b/src/drivers/ins/vectornav/libvnc/src/vn/protocol/spi.c new file mode 100644 index 0000000000..5aa381c82b --- /dev/null +++ b/src/drivers/ins/vectornav/libvnc/src/vn/protocol/spi.c @@ -0,0 +1,2911 @@ +#include "vn/protocol/spi.h" +#include +#include "vn/util.h" + +#define UNUSED(x) (void)(sizeof(x)) + +VnError VnSpi_genGenericCommand( + char cmdId, + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize) +{ + size_t i; + + if (*size < 1 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + buffer[0] = cmdId; + + for (i = 1; i < desiredLength; i++) + buffer[i] = 0x00; + + *responseSize = 2; + *size = desiredLength > 1 ? desiredLength : 1; + + return E_NONE; +} + +VnError VnSpi_genWriteSettings( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize) +{ + return VnSpi_genGenericCommand( + 3, + buffer, + size, + desiredLength, + responseSize); +} + +VnError VnSpi_genRestorFactorySettings( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize) +{ + return VnSpi_genGenericCommand( + 4, + buffer, + size, + desiredLength, + responseSize); +} + +VnError VnSpi_genTare( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize) +{ + return VnSpi_genGenericCommand( + 5, + buffer, + size, + desiredLength, + responseSize); +} + +VnError VnSpi_genReset( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize) +{ + return VnSpi_genGenericCommand( + 6, + buffer, + size, + desiredLength, + responseSize); +} + +VnError VnSpi_genRead( + char* buffer, + size_t* size, + uint8_t regId, + size_t desiredLength) +{ + size_t i; + + if (*size < 4 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + buffer[0] = 0x01; + buffer[1] = regId; + buffer[2] = 0x00; + buffer[3] = 0x00; + + for (i = 4; i < desiredLength; i++) + buffer[i] = 0x00; + + *size = desiredLength > 3 ? desiredLength : 3; + + return E_NONE; +} + +VnError VnSpi_parseUserTag( + const char* response, + char* tag, + size_t tagLength) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + if (tagLength < strlen(pos) + 1) + return E_BUFFER_TOO_SMALL; + + #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 + + strcpy(tag, pos); + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif + + return E_NONE; +} + +VnError VnSpi_parseModelNumber( + const char* response, + char* productName, + size_t productNameLength) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + if (productNameLength < strlen(pos) + 1) + return E_BUFFER_TOO_SMALL; + + #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 + + strcpy(productName, pos); + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif + + return E_NONE; +} + +VnError VnSpi_parseHardwareRevision( + const char* response, + uint32_t* revision) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *revision = VnUtil_extractUint32(pos); + pos += sizeof(uint32_t); + + return E_NONE; +} + +VnError VnSpi_parseSerialNumber( + const char* response, + uint32_t* serialNum) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *serialNum = VnUtil_extractUint32(pos); + pos += sizeof(uint32_t); + + return E_NONE; +} + +VnError VnSpi_parseFirmwareVersion( + const char* response, + char* firmwareVersion, + size_t firmwareVersionLength) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + if (firmwareVersionLength < strlen(pos) + 1) + return E_BUFFER_TOO_SMALL; + + #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 + + strcpy(firmwareVersion, pos); + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif + + return E_NONE; +} + +VnError VnSpi_parseSerialBaudRate( + const char* response, + uint32_t* baudrate) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *baudrate = VnUtil_extractUint32(pos); + pos += sizeof(uint32_t); + + return E_NONE; +} + +VnError VnSpi_parseAsyncDataOutputType( + const char* response, + uint32_t* ador) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *ador = VnUtil_extractUint32(pos); + pos += sizeof(uint32_t); + + return E_NONE; +} + +VnError VnSpi_parseAsyncDataOutputFrequency( + const char* response, + uint32_t* adof) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *adof = VnUtil_extractUint32(pos); + pos += sizeof(uint32_t); + + return E_NONE; +} + +VnError VnSpi_parseYawPitchRoll( + const char* response, + vec3f* yawPitchRoll) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *yawPitchRoll = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseAttitudeQuaternion( + const char* response, + vec4f* quat) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *quat = VnUtil_extractVec4f(pos); + pos += 4 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseQuaternionMagneticAccelerationAndAngularRates( + const char* response, + vec4f* quat, + vec3f* mag, + vec3f* accel, + vec3f* gyro) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *quat = VnUtil_extractVec4f(pos); + pos += 4 * sizeof(float); + *mag = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *accel = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *gyro = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseMagneticMeasurements( + const char* response, + vec3f* mag) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *mag = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseAccelerationMeasurements( + const char* response, + vec3f* accel) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *accel = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseAngularRateMeasurements( + const char* response, + vec3f* gyro) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *gyro = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseMagneticAccelerationAndAngularRates( + const char* response, + vec3f* mag, + vec3f* accel, + vec3f* gyro) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *mag = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *accel = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *gyro = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseMagneticAndGravityReferenceVectors( + const char* response, + vec3f* magRef, + vec3f* accRef) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *magRef = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *accRef = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseFilterMeasurementsVarianceParameters( + const char* response, + float* angularWalkVariance, + vec3f* angularRateVariance, + vec3f* magneticVariance, + vec3f* accelerationVariance) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *angularWalkVariance = VnUtil_extractFloat(pos); + pos += sizeof(float); + *angularRateVariance = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *magneticVariance = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *accelerationVariance = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseMagnetometerCompensation( + const char* response, + mat3f* c, + vec3f* b) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *c = VnUtil_extractMat3f(pos); + pos += 9 * sizeof(float); + *b = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseFilterActiveTuningParameters( + const char* response, + float* magneticDisturbanceGain, + float* accelerationDisturbanceGain, + float* magneticDisturbanceMemory, + float* accelerationDisturbanceMemory) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *magneticDisturbanceGain = VnUtil_extractFloat(pos); + pos += sizeof(float); + *accelerationDisturbanceGain = VnUtil_extractFloat(pos); + pos += sizeof(float); + *magneticDisturbanceMemory = VnUtil_extractFloat(pos); + pos += sizeof(float); + *accelerationDisturbanceMemory = VnUtil_extractFloat(pos); + pos += sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseAccelerationCompensation( + const char* response, + mat3f* c, + vec3f* b) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *c = VnUtil_extractMat3f(pos); + pos += 9 * sizeof(float); + *b = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseReferenceFrameRotation( + const char* response, + mat3f* c) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *c = VnUtil_extractMat3f(pos); + pos += 9 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseYawPitchRollMagneticAccelerationAndAngularRates( + const char* response, + vec3f* yawPitchRoll, + vec3f* mag, + vec3f* accel, + vec3f* gyro) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *yawPitchRoll = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *mag = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *accel = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *gyro = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseCommunicationProtocolControl( + const char* response, + uint8_t* serialCount, + uint8_t* serialStatus, + uint8_t* spiCount, + uint8_t* spiStatus, + uint8_t* serialChecksum, + uint8_t* spiChecksum, + uint8_t* errorMode) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *serialCount = (uint8_t) *pos; + pos += sizeof(uint8_t); + *serialStatus = (uint8_t) *pos; + pos += sizeof(uint8_t); + *spiCount = (uint8_t) *pos; + pos += sizeof(uint8_t); + *spiStatus = (uint8_t) *pos; + pos += sizeof(uint8_t); + *serialChecksum = (uint8_t) *pos; + pos += sizeof(uint8_t); + *spiChecksum = (uint8_t) *pos; + pos += sizeof(uint8_t); + *errorMode = (uint8_t) *pos; + pos += sizeof(uint8_t); + + return E_NONE; +} + +VnError VnSpi_parseSynchronizationControl( + const char* response, + uint8_t* syncInMode, + uint8_t* syncInEdge, + uint16_t* syncInSkipFactor, + uint32_t* reserved1, + uint8_t* syncOutMode, + uint8_t* syncOutPolarity, + uint16_t* syncOutSkipFactor, + uint32_t* syncOutPulseWidth, + uint32_t* reserved2) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *syncInMode = (uint8_t) *pos; + pos += sizeof(uint8_t); + *syncInEdge = (uint8_t) *pos; + pos += sizeof(uint8_t); + *syncInSkipFactor = VnUtil_extractUint16(pos); + pos += sizeof(uint16_t); + *reserved1 = VnUtil_extractUint32(pos); + pos += sizeof(uint32_t); + *syncOutMode = (uint8_t) *pos; + pos += sizeof(uint8_t); + *syncOutPolarity = (uint8_t) *pos; + pos += sizeof(uint8_t); + *syncOutSkipFactor = VnUtil_extractUint16(pos); + pos += sizeof(uint16_t); + *syncOutPulseWidth = VnUtil_extractUint32(pos); + pos += sizeof(uint32_t); + *reserved2 = VnUtil_extractUint32(pos); + pos += sizeof(uint32_t); + + return E_NONE; +} + +VnError VnSpi_parseSynchronizationStatus( + const char* response, + uint32_t* syncInCount, + uint32_t* syncInTime, + uint32_t* syncOutCount) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *syncInCount = VnUtil_extractUint32(pos); + pos += sizeof(uint32_t); + *syncInTime = VnUtil_extractUint32(pos); + pos += sizeof(uint32_t); + *syncOutCount = VnUtil_extractUint32(pos); + pos += sizeof(uint32_t); + + return E_NONE; +} + +VnError VnSpi_parseFilterBasicControl( + const char* response, + uint8_t* magMode, + uint8_t* extMagMode, + uint8_t* extAccMode, + uint8_t* extGyroMode, + vec3f* gyroLimit) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *magMode = (uint8_t) *pos; + pos += sizeof(uint8_t); + *extMagMode = (uint8_t) *pos; + pos += sizeof(uint8_t); + *extAccMode = (uint8_t) *pos; + pos += sizeof(uint8_t); + *extGyroMode = (uint8_t) *pos; + pos += sizeof(uint8_t); + *gyroLimit = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseVpeBasicControl( + const char* response, + uint8_t* enable, + uint8_t* headingMode, + uint8_t* filteringMode, + uint8_t* tuningMode) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *enable = (uint8_t) *pos; + pos += sizeof(uint8_t); + *headingMode = (uint8_t) *pos; + pos += sizeof(uint8_t); + *filteringMode = (uint8_t) *pos; + pos += sizeof(uint8_t); + *tuningMode = (uint8_t) *pos; + pos += sizeof(uint8_t); + + return E_NONE; +} + +VnError VnSpi_parseVpeMagnetometerBasicTuning( + const char* response, + vec3f* baseTuning, + vec3f* adaptiveTuning, + vec3f* adaptiveFiltering) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *baseTuning = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *adaptiveTuning = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *adaptiveFiltering = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseVpeMagnetometerAdvancedTuning( + const char* response, + vec3f* minFiltering, + vec3f* maxFiltering, + float* maxAdaptRate, + float* disturbanceWindow, + float* maxTuning) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *minFiltering = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *maxFiltering = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *maxAdaptRate = VnUtil_extractFloat(pos); + pos += sizeof(float); + *disturbanceWindow = VnUtil_extractFloat(pos); + pos += sizeof(float); + *maxTuning = VnUtil_extractFloat(pos); + pos += sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseVpeAccelerometerBasicTuning( + const char* response, + vec3f* baseTuning, + vec3f* adaptiveTuning, + vec3f* adaptiveFiltering) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *baseTuning = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *adaptiveTuning = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *adaptiveFiltering = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseVpeAccelerometerAdvancedTuning( + const char* response, + vec3f* minFiltering, + vec3f* maxFiltering, + float* maxAdaptRate, + float* disturbanceWindow, + float* maxTuning) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *minFiltering = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *maxFiltering = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *maxAdaptRate = VnUtil_extractFloat(pos); + pos += sizeof(float); + *disturbanceWindow = VnUtil_extractFloat(pos); + pos += sizeof(float); + *maxTuning = VnUtil_extractFloat(pos); + pos += sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseVpeGyroBasicTuning( + const char* response, + vec3f* angularWalkVariance, + vec3f* baseTuning, + vec3f* adaptiveTuning) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *angularWalkVariance = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *baseTuning = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *adaptiveTuning = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseFilterStartupGyroBias( + const char* response, + vec3f* bias) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *bias = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseMagnetometerCalibrationControl( + const char* response, + uint8_t* hsiMode, + uint8_t* hsiOutput, + uint8_t* convergeRate) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *hsiMode = (uint8_t) *pos; + pos += sizeof(uint8_t); + *hsiOutput = (uint8_t) *pos; + pos += sizeof(uint8_t); + *convergeRate = (uint8_t) *pos; + pos += sizeof(uint8_t); + + return E_NONE; +} + +VnError VnSpi_parseCalculatedMagnetometerCalibration( + const char* response, + mat3f* c, + vec3f* b) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *c = VnUtil_extractMat3f(pos); + pos += 9 * sizeof(float); + *b = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseIndoorHeadingModeControl( + const char* response, + float* maxRateError, + uint8_t* reserved1) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *maxRateError = VnUtil_extractFloat(pos); + pos += sizeof(float); + *reserved1 = (uint8_t) *pos; + pos += sizeof(uint8_t); + + return E_NONE; +} + +VnError VnSpi_parseVelocityCompensationMeasurement( + const char* response, + vec3f* velocity) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *velocity = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseVelocityCompensationControl( + const char* response, + uint8_t* mode, + float* velocityTuning, + float* rateTuning) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *mode = (uint8_t) *pos; + pos += sizeof(uint8_t); + *velocityTuning = VnUtil_extractFloat(pos); + pos += sizeof(float); + *rateTuning = VnUtil_extractFloat(pos); + pos += sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseVelocityCompensationStatus( + const char* response, + float* x, + float* xDot, + vec3f* accelOffset, + vec3f* omega) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *x = VnUtil_extractFloat(pos); + pos += sizeof(float); + *xDot = VnUtil_extractFloat(pos); + pos += sizeof(float); + *accelOffset = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *omega = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseImuMeasurements( + const char* response, + vec3f* mag, + vec3f* accel, + vec3f* gyro, + float* temp, + float* pressure) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *mag = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *accel = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *gyro = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *temp = VnUtil_extractFloat(pos); + pos += sizeof(float); + *pressure = VnUtil_extractFloat(pos); + pos += sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseGpsConfiguration( + const char* response, + uint8_t* mode, + uint8_t* ppsSource, + uint8_t* reserved1, + uint8_t* reserved2, + uint8_t* reserved3) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *mode = (uint8_t) *pos; + pos += sizeof(uint8_t); + *ppsSource = (uint8_t) *pos; + pos += sizeof(uint8_t); + *reserved1 = (uint8_t) *pos; + pos += sizeof(uint8_t); + *reserved2 = (uint8_t) *pos; + pos += sizeof(uint8_t); + *reserved3 = (uint8_t) *pos; + pos += sizeof(uint8_t); + + return E_NONE; +} + +VnError VnSpi_parseGpsAntennaOffset( + const char* response, + vec3f* position) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *position = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseGpsSolutionLla( + const char* response, + double* time, + uint16_t* week, + uint8_t* gpsFix, + uint8_t* numSats, + vec3d* lla, + vec3f* nedVel, + vec3f* nedAcc, + float* speedAcc, + float* timeAcc) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *time = VnUtil_extractDouble(pos); + pos += sizeof(double); + *week = VnUtil_extractUint16(pos); + pos += sizeof(uint16_t); + *gpsFix = (uint8_t) *pos; + pos += sizeof(uint8_t); + *numSats = (uint8_t) *pos; + pos += sizeof(uint8_t); + pos += 4; + *lla = VnUtil_extractVec3d(pos); + pos += 3 * sizeof(double); + *nedVel = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *nedAcc = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *speedAcc = VnUtil_extractFloat(pos); + pos += sizeof(float); + *timeAcc = VnUtil_extractFloat(pos); + pos += sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseGpsSolutionEcef( + const char* response, + double* tow, + uint16_t* week, + uint8_t* gpsFix, + uint8_t* numSats, + vec3d* position, + vec3f* velocity, + vec3f* posAcc, + float* speedAcc, + float* timeAcc) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *tow = VnUtil_extractDouble(pos); + pos += sizeof(double); + *week = VnUtil_extractUint16(pos); + pos += sizeof(uint16_t); + *gpsFix = (uint8_t) *pos; + pos += sizeof(uint8_t); + *numSats = (uint8_t) *pos; + pos += sizeof(uint8_t); + pos += 4; + *position = VnUtil_extractVec3d(pos); + pos += 3 * sizeof(double); + *velocity = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *posAcc = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *speedAcc = VnUtil_extractFloat(pos); + pos += sizeof(float); + *timeAcc = VnUtil_extractFloat(pos); + pos += sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseInsSolutionLla( + const char* response, + double* time, + uint16_t* week, + uint16_t* status, + vec3f* yawPitchRoll, + vec3d* position, + vec3f* nedVel, + float* attUncertainty, + float* posUncertainty, + float* velUncertainty) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *time = VnUtil_extractDouble(pos); + pos += sizeof(double); + *week = VnUtil_extractUint16(pos); + pos += sizeof(uint16_t); + /* Use this cast to avoid a compile warning. */ + UNUSED(status); + *yawPitchRoll = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *position = VnUtil_extractVec3d(pos); + pos += 3 * sizeof(double); + *nedVel = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *attUncertainty = VnUtil_extractFloat(pos); + pos += sizeof(float); + *posUncertainty = VnUtil_extractFloat(pos); + pos += sizeof(float); + *velUncertainty = VnUtil_extractFloat(pos); + pos += sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseInsSolutionEcef( + const char* response, + double* time, + uint16_t* week, + uint16_t* status, + vec3f* yawPitchRoll, + vec3d* position, + vec3f* velocity, + float* attUncertainty, + float* posUncertainty, + float* velUncertainty) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *time = VnUtil_extractDouble(pos); + pos += sizeof(double); + *week = VnUtil_extractUint16(pos); + pos += sizeof(uint16_t); + /* Use this cast to avoid a compile warning. */ + UNUSED(status); + *yawPitchRoll = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *position = VnUtil_extractVec3d(pos); + pos += 3 * sizeof(double); + *velocity = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *attUncertainty = VnUtil_extractFloat(pos); + pos += sizeof(float); + *posUncertainty = VnUtil_extractFloat(pos); + pos += sizeof(float); + *velUncertainty = VnUtil_extractFloat(pos); + pos += sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseInsBasicConfiguration( + const char* response, + uint8_t* scenario, + uint8_t* ahrsAiding, + uint8_t* estBaseline, + uint8_t* resv2) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *scenario = (uint8_t) *pos; + pos += sizeof(uint8_t); + *ahrsAiding = (uint8_t) *pos; + pos += sizeof(uint8_t); + *estBaseline = (uint8_t) *pos; + pos += sizeof(uint8_t); + *resv2 = (uint8_t) *pos; + pos += sizeof(uint8_t); + + return E_NONE; +} + +VnError VnSpi_parseInsAdvancedConfiguration( + const char* response, + uint8_t* useMag, + uint8_t* usePres, + uint8_t* posAtt, + uint8_t* velAtt, + uint8_t* velBias, + uint8_t* useFoam, + uint8_t* gpsCovType, + uint8_t* velCount, + float* velInit, + float* moveOrigin, + float* gpsTimeout, + float* deltaLimitPos, + float* deltaLimitVel, + float* minPosUncertainty, + float* minVelUncertainty) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *useMag = (uint8_t) *pos; + pos += sizeof(uint8_t); + *usePres = (uint8_t) *pos; + pos += sizeof(uint8_t); + *posAtt = (uint8_t) *pos; + pos += sizeof(uint8_t); + *velAtt = (uint8_t) *pos; + pos += sizeof(uint8_t); + *velBias = (uint8_t) *pos; + pos += sizeof(uint8_t); + *useFoam = (uint8_t) *pos; + pos += sizeof(uint8_t); + *gpsCovType = (uint8_t) *pos; + pos += sizeof(uint8_t); + *velCount = (uint8_t) *pos; + pos += sizeof(uint8_t); + *velInit = VnUtil_extractFloat(pos); + pos += sizeof(float); + *moveOrigin = VnUtil_extractFloat(pos); + pos += sizeof(float); + *gpsTimeout = VnUtil_extractFloat(pos); + pos += sizeof(float); + *deltaLimitPos = VnUtil_extractFloat(pos); + pos += sizeof(float); + *deltaLimitVel = VnUtil_extractFloat(pos); + pos += sizeof(float); + *minPosUncertainty = VnUtil_extractFloat(pos); + pos += sizeof(float); + *minVelUncertainty = VnUtil_extractFloat(pos); + pos += sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseInsStateLla( + const char* response, + vec3f* yawPitchRoll, + vec3d* position, + vec3f* velocity, + vec3f* accel, + vec3f* angularRate) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *yawPitchRoll = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *position = VnUtil_extractVec3d(pos); + pos += 3 * sizeof(double); + *velocity = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *accel = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *angularRate = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseInsStateEcef( + const char* response, + vec3f* yawPitchRoll, + vec3d* position, + vec3f* velocity, + vec3f* accel, + vec3f* angularRate) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *yawPitchRoll = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *position = VnUtil_extractVec3d(pos); + pos += 3 * sizeof(double); + *velocity = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *accel = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *angularRate = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseStartupFilterBiasEstimate( + const char* response, + vec3f* gyroBias, + vec3f* accelBias, + float* pressureBias) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *gyroBias = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *accelBias = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *pressureBias = VnUtil_extractFloat(pos); + pos += sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseDeltaThetaAndDeltaVelocity( + const char* response, + float* deltaTime, + vec3f* deltaTheta, + vec3f* deltaVelocity) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *deltaTime = VnUtil_extractFloat(pos); + pos += sizeof(float); + *deltaTheta = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *deltaVelocity = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseDeltaThetaAndDeltaVelocityConfiguration( + const char* response, + uint8_t* integrationFrame, + uint8_t* gyroCompensation, + uint8_t* accelCompensation, + uint8_t* reserved1, + uint16_t* reserved2) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *integrationFrame = (uint8_t) *pos; + pos += sizeof(uint8_t); + *gyroCompensation = (uint8_t) *pos; + pos += sizeof(uint8_t); + *accelCompensation = (uint8_t) *pos; + pos += sizeof(uint8_t); + *reserved1 = (uint8_t) *pos; + pos += sizeof(uint8_t); + *reserved2 = VnUtil_extractUint16(pos); + pos += sizeof(uint16_t); + + return E_NONE; +} + +VnError VnSpi_parseReferenceVectorConfiguration( + const char* response, + uint8_t* useMagModel, + uint8_t* useGravityModel, + uint8_t* resv1, + uint8_t* resv2, + uint32_t* recalcThreshold, + float* year, + vec3d* position) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *useMagModel = (uint8_t) *pos; + pos += sizeof(uint8_t); + *useGravityModel = (uint8_t) *pos; + pos += sizeof(uint8_t); + *resv1 = (uint8_t) *pos; + pos += sizeof(uint8_t); + *resv2 = (uint8_t) *pos; + pos += sizeof(uint8_t); + *recalcThreshold = VnUtil_extractUint32(pos); + pos += sizeof(uint32_t); + *year = VnUtil_extractFloat(pos); + pos += sizeof(float); + pos += 4; + *position = VnUtil_extractVec3d(pos); + pos += 3 * sizeof(double); + + return E_NONE; +} + +VnError VnSpi_parseGyroCompensation( + const char* response, + mat3f* c, + vec3f* b) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *c = VnUtil_extractMat3f(pos); + pos += 9 * sizeof(float); + *b = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseImuFilteringConfiguration( + const char* response, + uint16_t* magWindowSize, + uint16_t* accelWindowSize, + uint16_t* gyroWindowSize, + uint16_t* tempWindowSize, + uint16_t* presWindowSize, + uint8_t* magFilterMode, + uint8_t* accelFilterMode, + uint8_t* gyroFilterMode, + uint8_t* tempFilterMode, + uint8_t* presFilterMode) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *magWindowSize = VnUtil_extractUint16(pos); + pos += sizeof(uint16_t); + *accelWindowSize = VnUtil_extractUint16(pos); + pos += sizeof(uint16_t); + *gyroWindowSize = VnUtil_extractUint16(pos); + pos += sizeof(uint16_t); + *tempWindowSize = VnUtil_extractUint16(pos); + pos += sizeof(uint16_t); + *presWindowSize = VnUtil_extractUint16(pos); + pos += sizeof(uint16_t); + *magFilterMode = (uint8_t) *pos; + pos += sizeof(uint8_t); + *accelFilterMode = (uint8_t) *pos; + pos += sizeof(uint8_t); + *gyroFilterMode = (uint8_t) *pos; + pos += sizeof(uint8_t); + *tempFilterMode = (uint8_t) *pos; + pos += sizeof(uint8_t); + *presFilterMode = (uint8_t) *pos; + pos += sizeof(uint8_t); + + return E_NONE; +} + +VnError VnSpi_parseGpsCompassBaseline( + const char* response, + vec3f* position, + vec3f* uncertainty) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *position = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *uncertainty = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseGpsCompassEstimatedBaseline( + const char* response, + uint8_t* estBaselineUsed, + uint8_t* resv, + uint16_t* numMeas, + vec3f* position, + vec3f* uncertainty) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *estBaselineUsed = (uint8_t) *pos; + pos += sizeof(uint8_t); + *resv = (uint8_t) *pos; + pos += sizeof(uint8_t); + *numMeas = VnUtil_extractUint16(pos); + pos += sizeof(uint16_t); + *position = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *uncertainty = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseImuRateConfiguration( + const char* response, + uint16_t* imuRate, + uint16_t* navDivisor, + float* filterTargetRate, + float* filterMinRate) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *imuRate = VnUtil_extractUint16(pos); + pos += sizeof(uint16_t); + *navDivisor = VnUtil_extractUint16(pos); + pos += sizeof(uint16_t); + *filterTargetRate = VnUtil_extractFloat(pos); + pos += sizeof(float); + *filterMinRate = VnUtil_extractFloat(pos); + pos += sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseYawPitchRollTrueBodyAccelerationAndAngularRates( + const char* response, + vec3f* yawPitchRoll, + vec3f* bodyAccel, + vec3f* gyro) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *yawPitchRoll = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *bodyAccel = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *gyro = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_parseYawPitchRollTrueInertialAccelerationAndAngularRates( + const char* response, + vec3f* yawPitchRoll, + vec3f* inertialAccel, + vec3f* gyro) +{ + const char* pos = response + 3; + + if (*pos != 0) + return (VnError)(*pos + E_SENSOR_HARD_FAULT - 1); + + pos++; + + *yawPitchRoll = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *inertialAccel = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + *gyro = VnUtil_extractVec3f(pos); + pos += 3 * sizeof(float); + + return E_NONE; +} + +VnError VnSpi_genReadUserTag(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 4; + + return VnSpi_genRead(buffer, size, 0, desiredLength); +} + +VnError VnSpi_genReadModelNumber(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 4; + + return VnSpi_genRead(buffer, size, 1, desiredLength); +} + +VnError VnSpi_genReadHardwareRevision(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 8; + + return VnSpi_genRead(buffer, size, 2, desiredLength); +} + +VnError VnSpi_genReadSerialNumber(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 8; + + return VnSpi_genRead(buffer, size, 3, desiredLength); +} + +VnError VnSpi_genReadFirmwareVersion(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 4; + + return VnSpi_genRead(buffer, size, 4, desiredLength); +} + +VnError VnSpi_genReadSerialBaudRate(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 9; + + return VnSpi_genRead(buffer, size, 5, desiredLength); +} + +VnError VnSpi_genReadAsyncDataOutputType(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 9; + + return VnSpi_genRead(buffer, size, 6, desiredLength); +} + +VnError VnSpi_genReadAsyncDataOutputFrequency(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 9; + + return VnSpi_genRead(buffer, size, 7, desiredLength); +} + +VnError VnSpi_genReadYawPitchRoll(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 16; + + return VnSpi_genRead(buffer, size, 8, desiredLength); +} + +VnError VnSpi_genReadAttitudeQuaternion(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 20; + + return VnSpi_genRead(buffer, size, 9, desiredLength); +} + +VnError VnSpi_genReadQuaternionMagneticAccelerationAndAngularRates(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 56; + + return VnSpi_genRead(buffer, size, 15, desiredLength); +} + +VnError VnSpi_genReadMagneticMeasurements(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 16; + + return VnSpi_genRead(buffer, size, 17, desiredLength); +} + +VnError VnSpi_genReadAccelerationMeasurements(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 16; + + return VnSpi_genRead(buffer, size, 18, desiredLength); +} + +VnError VnSpi_genReadAngularRateMeasurements(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 16; + + return VnSpi_genRead(buffer, size, 19, desiredLength); +} + +VnError VnSpi_genReadMagneticAccelerationAndAngularRates(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 40; + + return VnSpi_genRead(buffer, size, 20, desiredLength); +} + +VnError VnSpi_genReadMagneticAndGravityReferenceVectors(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 28; + + return VnSpi_genRead(buffer, size, 21, desiredLength); +} + +VnError VnSpi_genReadMagnetometerCompensation(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 52; + + return VnSpi_genRead(buffer, size, 23, desiredLength); +} + +VnError VnSpi_genReadAccelerationCompensation(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 52; + + return VnSpi_genRead(buffer, size, 25, desiredLength); +} + +VnError VnSpi_genReadReferenceFrameRotation(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 40; + + return VnSpi_genRead(buffer, size, 26, desiredLength); +} + +VnError VnSpi_genReadYawPitchRollMagneticAccelerationAndAngularRates(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 52; + + return VnSpi_genRead(buffer, size, 27, desiredLength); +} + +VnError VnSpi_genReadCommunicationProtocolControl(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 11; + + return VnSpi_genRead(buffer, size, 30, desiredLength); +} + +VnError VnSpi_genReadSynchronizationControl(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 24; + + return VnSpi_genRead(buffer, size, 32, desiredLength); +} + +VnError VnSpi_genReadSynchronizationStatus(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 16; + + return VnSpi_genRead(buffer, size, 33, desiredLength); +} + +VnError VnSpi_genReadVpeBasicControl(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 8; + + return VnSpi_genRead(buffer, size, 35, desiredLength); +} + +VnError VnSpi_genReadVpeMagnetometerBasicTuning(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 40; + + return VnSpi_genRead(buffer, size, 36, desiredLength); +} + +VnError VnSpi_genReadVpeAccelerometerBasicTuning(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 40; + + return VnSpi_genRead(buffer, size, 38, desiredLength); +} + +VnError VnSpi_genReadMagnetometerCalibrationControl(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 7; + + return VnSpi_genRead(buffer, size, 44, desiredLength); +} + +VnError VnSpi_genReadCalculatedMagnetometerCalibration(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 52; + + return VnSpi_genRead(buffer, size, 47, desiredLength); +} + +VnError VnSpi_genReadVelocityCompensationMeasurement(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 16; + + return VnSpi_genRead(buffer, size, 50, desiredLength); +} + +VnError VnSpi_genReadVelocityCompensationControl(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 13; + + return VnSpi_genRead(buffer, size, 51, desiredLength); +} + +VnError VnSpi_genReadImuMeasurements(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 48; + + return VnSpi_genRead(buffer, size, 54, desiredLength); +} + +VnError VnSpi_genReadGpsConfiguration(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 9; + + return VnSpi_genRead(buffer, size, 55, desiredLength); +} + +VnError VnSpi_genReadGpsAntennaOffset(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 16; + + return VnSpi_genRead(buffer, size, 57, desiredLength); +} + +VnError VnSpi_genReadGpsSolutionLla(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 76; + + return VnSpi_genRead(buffer, size, 58, desiredLength); +} + +VnError VnSpi_genReadGpsSolutionEcef(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 76; + + return VnSpi_genRead(buffer, size, 59, desiredLength); +} + +VnError VnSpi_genReadInsSolutionLla(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 76; + + return VnSpi_genRead(buffer, size, 63, desiredLength); +} + +VnError VnSpi_genReadInsSolutionEcef(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 76; + + return VnSpi_genRead(buffer, size, 64, desiredLength); +} + +VnError VnSpi_genReadInsBasicConfiguration(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 8; + + return VnSpi_genRead(buffer, size, 67, desiredLength); +} + +VnError VnSpi_genReadInsStateLla(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 76; + + return VnSpi_genRead(buffer, size, 72, desiredLength); +} + +VnError VnSpi_genReadInsStateEcef(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 76; + + return VnSpi_genRead(buffer, size, 73, desiredLength); +} + +VnError VnSpi_genReadStartupFilterBiasEstimate(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 32; + + return VnSpi_genRead(buffer, size, 74, desiredLength); +} + +VnError VnSpi_genReadDeltaThetaAndDeltaVelocity(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 32; + + return VnSpi_genRead(buffer, size, 80, desiredLength); +} + +VnError VnSpi_genReadDeltaThetaAndDeltaVelocityConfiguration(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 10; + + return VnSpi_genRead(buffer, size, 82, desiredLength); +} + +VnError VnSpi_genReadReferenceVectorConfiguration(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 44; + + return VnSpi_genRead(buffer, size, 83, desiredLength); +} + +VnError VnSpi_genReadGyroCompensation(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 52; + + return VnSpi_genRead(buffer, size, 84, desiredLength); +} + +VnError VnSpi_genReadImuFilteringConfiguration(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 19; + + return VnSpi_genRead(buffer, size, 85, desiredLength); +} + +VnError VnSpi_genReadGpsCompassBaseline(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 28; + + return VnSpi_genRead(buffer, size, 93, desiredLength); +} + +VnError VnSpi_genReadGpsCompassEstimatedBaseline(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 32; + + return VnSpi_genRead(buffer, size, 97, desiredLength); +} + +VnError VnSpi_genReadYawPitchRollTrueBodyAccelerationAndAngularRates(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 40; + + return VnSpi_genRead(buffer, size, 239, desiredLength); +} + +VnError VnSpi_genReadYawPitchRollTrueInertialAccelerationAndAngularRates(char* buffer, size_t* size, size_t desiredLength, size_t* responseSize) +{ + *responseSize = 40; + + return VnSpi_genRead(buffer, size, 240, desiredLength); +} + +VnError VnSpi_genWriteUserTag( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + char* tag) +{ + char* pos = buffer; + + if (*size < 4 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 4; + + *pos++ = 2; + *pos++ = 0; + *pos++ = 0; + *pos++ = 0; + memcpy(pos, &tag, strlen(tag)); + pos += strlen(tag); + + return E_NONE; +} + +VnError VnSpi_genWriteSerialBaudRate( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint32_t baudrate) +{ + char* pos = buffer; + + if (*size < 9 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 9; + + *pos++ = 2; + *pos++ = 5; + *pos++ = 0; + *pos++ = 0; + baudrate = htos32(baudrate); + memcpy(pos, &baudrate, sizeof(uint32_t)); + pos += sizeof(uint32_t); + + return E_NONE; +} + +VnError VnSpi_genWriteAsyncDataOutputType( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint32_t ador) +{ + char* pos = buffer; + + if (*size < 9 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 9; + + *pos++ = 2; + *pos++ = 6; + *pos++ = 0; + *pos++ = 0; + ador = htos32(ador); + memcpy(pos, &ador, sizeof(uint32_t)); + pos += sizeof(uint32_t); + + return E_NONE; +} + +VnError VnSpi_genWriteAsyncDataOutputFrequency( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint32_t adof) +{ + char* pos = buffer; + + if (*size < 9 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 9; + + *pos++ = 2; + *pos++ = 7; + *pos++ = 0; + *pos++ = 0; + adof = htos32(adof); + memcpy(pos, &adof, sizeof(uint32_t)); + pos += sizeof(uint32_t); + + return E_NONE; +} + +VnError VnSpi_genWriteMagneticAndGravityReferenceVectors( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + vec3f magRef, + vec3f accRef) +{ + char* pos = buffer; + + if (*size < 28 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 28; + + *pos++ = 2; + *pos++ = 21; + *pos++ = 0; + *pos++ = 0; + magRef.c[0] = htosf4(magRef.c[0]); + magRef.c[1] = htosf4(magRef.c[1]); + magRef.c[2] = htosf4(magRef.c[2]); + memcpy(pos, &magRef, sizeof(vec3f)); + pos += sizeof(vec3f); + accRef.c[0] = htosf4(accRef.c[0]); + accRef.c[1] = htosf4(accRef.c[1]); + accRef.c[2] = htosf4(accRef.c[2]); + memcpy(pos, &accRef, sizeof(vec3f)); + pos += sizeof(vec3f); + + return E_NONE; +} + +VnError VnSpi_genWriteMagnetometerCompensation( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + mat3f c, + vec3f b) +{ + char* pos = buffer; + + if (*size < 52 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 52; + + *pos++ = 2; + *pos++ = 23; + *pos++ = 0; + *pos++ = 0; + c.e[0] = htosf4(c.e[0]); + c.e[1] = htosf4(c.e[1]); + c.e[2] = htosf4(c.e[2]); + c.e[3] = htosf4(c.e[3]); + c.e[4] = htosf4(c.e[4]); + c.e[5] = htosf4(c.e[5]); + c.e[6] = htosf4(c.e[6]); + c.e[7] = htosf4(c.e[7]); + c.e[8] = htosf4(c.e[8]); + memcpy(pos, &c, sizeof(mat3f)); + pos += sizeof(mat3f); + b.c[0] = htosf4(b.c[0]); + b.c[1] = htosf4(b.c[1]); + b.c[2] = htosf4(b.c[2]); + memcpy(pos, &b, sizeof(vec3f)); + pos += sizeof(vec3f); + + return E_NONE; +} + +VnError VnSpi_genWriteAccelerationCompensation( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + mat3f c, + vec3f b) +{ + char* pos = buffer; + + if (*size < 52 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 52; + + *pos++ = 2; + *pos++ = 25; + *pos++ = 0; + *pos++ = 0; + c.e[0] = htosf4(c.e[0]); + c.e[1] = htosf4(c.e[1]); + c.e[2] = htosf4(c.e[2]); + c.e[3] = htosf4(c.e[3]); + c.e[4] = htosf4(c.e[4]); + c.e[5] = htosf4(c.e[5]); + c.e[6] = htosf4(c.e[6]); + c.e[7] = htosf4(c.e[7]); + c.e[8] = htosf4(c.e[8]); + memcpy(pos, &c, sizeof(mat3f)); + pos += sizeof(mat3f); + b.c[0] = htosf4(b.c[0]); + b.c[1] = htosf4(b.c[1]); + b.c[2] = htosf4(b.c[2]); + memcpy(pos, &b, sizeof(vec3f)); + pos += sizeof(vec3f); + + return E_NONE; +} + +VnError VnSpi_genWriteReferenceFrameRotation( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + mat3f c) +{ + char* pos = buffer; + + if (*size < 40 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 40; + + *pos++ = 2; + *pos++ = 26; + *pos++ = 0; + *pos++ = 0; + c.e[0] = htosf4(c.e[0]); + c.e[1] = htosf4(c.e[1]); + c.e[2] = htosf4(c.e[2]); + c.e[3] = htosf4(c.e[3]); + c.e[4] = htosf4(c.e[4]); + c.e[5] = htosf4(c.e[5]); + c.e[6] = htosf4(c.e[6]); + c.e[7] = htosf4(c.e[7]); + c.e[8] = htosf4(c.e[8]); + memcpy(pos, &c, sizeof(mat3f)); + pos += sizeof(mat3f); + + return E_NONE; +} + +VnError VnSpi_genWriteCommunicationProtocolControl( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint8_t serialCount, + uint8_t serialStatus, + uint8_t spiCount, + uint8_t spiStatus, + uint8_t serialChecksum, + uint8_t spiChecksum, + uint8_t errorMode) +{ + char* pos = buffer; + + if (*size < 11 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 11; + + *pos++ = 2; + *pos++ = 30; + *pos++ = 0; + *pos++ = 0; + *pos++ = serialCount; + *pos++ = serialStatus; + *pos++ = spiCount; + *pos++ = spiStatus; + *pos++ = serialChecksum; + *pos++ = spiChecksum; + *pos++ = errorMode; + + return E_NONE; +} + +VnError VnSpi_genWriteSynchronizationControl( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint8_t syncInMode, + uint8_t syncInEdge, + uint16_t syncInSkipFactor, + uint32_t reserved1, + uint8_t syncOutMode, + uint8_t syncOutPolarity, + uint16_t syncOutSkipFactor, + uint32_t syncOutPulseWidth, + uint32_t reserved2) +{ + char* pos = buffer; + + if (*size < 24 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 24; + + *pos++ = 2; + *pos++ = 32; + *pos++ = 0; + *pos++ = 0; + *pos++ = syncInMode; + *pos++ = syncInEdge; + syncInSkipFactor = htos16(syncInSkipFactor); + memcpy(pos, &syncInSkipFactor, sizeof(uint16_t)); + pos += sizeof(uint16_t); + reserved1 = htos32(reserved1); + memcpy(pos, &reserved1, sizeof(uint32_t)); + pos += sizeof(uint32_t); + *pos++ = syncOutMode; + *pos++ = syncOutPolarity; + syncOutSkipFactor = htos16(syncOutSkipFactor); + memcpy(pos, &syncOutSkipFactor, sizeof(uint16_t)); + pos += sizeof(uint16_t); + syncOutPulseWidth = htos32(syncOutPulseWidth); + memcpy(pos, &syncOutPulseWidth, sizeof(uint32_t)); + pos += sizeof(uint32_t); + reserved2 = htos32(reserved2); + memcpy(pos, &reserved2, sizeof(uint32_t)); + pos += sizeof(uint32_t); + + return E_NONE; +} + +VnError VnSpi_genWriteSynchronizationStatus( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint32_t syncInCount, + uint32_t syncInTime, + uint32_t syncOutCount) +{ + char* pos = buffer; + + if (*size < 16 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 16; + + *pos++ = 2; + *pos++ = 33; + *pos++ = 0; + *pos++ = 0; + syncInCount = htos32(syncInCount); + memcpy(pos, &syncInCount, sizeof(uint32_t)); + pos += sizeof(uint32_t); + syncInTime = htos32(syncInTime); + memcpy(pos, &syncInTime, sizeof(uint32_t)); + pos += sizeof(uint32_t); + syncOutCount = htos32(syncOutCount); + memcpy(pos, &syncOutCount, sizeof(uint32_t)); + pos += sizeof(uint32_t); + + return E_NONE; +} + +VnError VnSpi_genWriteVpeBasicControl( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint8_t enable, + uint8_t headingMode, + uint8_t filteringMode, + uint8_t tuningMode) +{ + char* pos = buffer; + + if (*size < 8 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 8; + + *pos++ = 2; + *pos++ = 35; + *pos++ = 0; + *pos++ = 0; + *pos++ = enable; + *pos++ = headingMode; + *pos++ = filteringMode; + *pos++ = tuningMode; + + return E_NONE; +} + +VnError VnSpi_genWriteVpeMagnetometerBasicTuning( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + vec3f baseTuning, + vec3f adaptiveTuning, + vec3f adaptiveFiltering) +{ + char* pos = buffer; + + if (*size < 40 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 40; + + *pos++ = 2; + *pos++ = 36; + *pos++ = 0; + *pos++ = 0; + baseTuning.c[0] = htosf4(baseTuning.c[0]); + baseTuning.c[1] = htosf4(baseTuning.c[1]); + baseTuning.c[2] = htosf4(baseTuning.c[2]); + memcpy(pos, &baseTuning, sizeof(vec3f)); + pos += sizeof(vec3f); + adaptiveTuning.c[0] = htosf4(adaptiveTuning.c[0]); + adaptiveTuning.c[1] = htosf4(adaptiveTuning.c[1]); + adaptiveTuning.c[2] = htosf4(adaptiveTuning.c[2]); + memcpy(pos, &adaptiveTuning, sizeof(vec3f)); + pos += sizeof(vec3f); + adaptiveFiltering.c[0] = htosf4(adaptiveFiltering.c[0]); + adaptiveFiltering.c[1] = htosf4(adaptiveFiltering.c[1]); + adaptiveFiltering.c[2] = htosf4(adaptiveFiltering.c[2]); + memcpy(pos, &adaptiveFiltering, sizeof(vec3f)); + pos += sizeof(vec3f); + + return E_NONE; +} + +VnError VnSpi_genWriteVpeAccelerometerBasicTuning( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + vec3f baseTuning, + vec3f adaptiveTuning, + vec3f adaptiveFiltering) +{ + char* pos = buffer; + + if (*size < 40 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 40; + + *pos++ = 2; + *pos++ = 38; + *pos++ = 0; + *pos++ = 0; + baseTuning.c[0] = htosf4(baseTuning.c[0]); + baseTuning.c[1] = htosf4(baseTuning.c[1]); + baseTuning.c[2] = htosf4(baseTuning.c[2]); + memcpy(pos, &baseTuning, sizeof(vec3f)); + pos += sizeof(vec3f); + adaptiveTuning.c[0] = htosf4(adaptiveTuning.c[0]); + adaptiveTuning.c[1] = htosf4(adaptiveTuning.c[1]); + adaptiveTuning.c[2] = htosf4(adaptiveTuning.c[2]); + memcpy(pos, &adaptiveTuning, sizeof(vec3f)); + pos += sizeof(vec3f); + adaptiveFiltering.c[0] = htosf4(adaptiveFiltering.c[0]); + adaptiveFiltering.c[1] = htosf4(adaptiveFiltering.c[1]); + adaptiveFiltering.c[2] = htosf4(adaptiveFiltering.c[2]); + memcpy(pos, &adaptiveFiltering, sizeof(vec3f)); + pos += sizeof(vec3f); + + return E_NONE; +} + +VnError VnSpi_genWriteMagnetometerCalibrationControl( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint8_t hsiMode, + uint8_t hsiOutput, + uint8_t convergeRate) +{ + char* pos = buffer; + + if (*size < 7 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 7; + + *pos++ = 2; + *pos++ = 44; + *pos++ = 0; + *pos++ = 0; + *pos++ = hsiMode; + *pos++ = hsiOutput; + *pos++ = convergeRate; + + return E_NONE; +} + +VnError VnSpi_genWriteVelocityCompensationMeasurement( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + vec3f velocity) +{ + char* pos = buffer; + + if (*size < 16 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 16; + + *pos++ = 2; + *pos++ = 50; + *pos++ = 0; + *pos++ = 0; + velocity.c[0] = htosf4(velocity.c[0]); + velocity.c[1] = htosf4(velocity.c[1]); + velocity.c[2] = htosf4(velocity.c[2]); + memcpy(pos, &velocity, sizeof(vec3f)); + pos += sizeof(vec3f); + + return E_NONE; +} + +VnError VnSpi_genWriteVelocityCompensationControl( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint8_t mode, + float velocityTuning, + float rateTuning) +{ + char* pos = buffer; + + if (*size < 13 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 13; + + *pos++ = 2; + *pos++ = 51; + *pos++ = 0; + *pos++ = 0; + *pos++ = mode; + velocityTuning = htosf4(velocityTuning); + memcpy(pos, &velocityTuning, sizeof(float)); + pos += sizeof(float); + rateTuning = htosf4(rateTuning); + memcpy(pos, &rateTuning, sizeof(float)); + pos += sizeof(float); + + return E_NONE; +} + +VnError VnSpi_genWriteGpsConfiguration( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint8_t mode, + uint8_t ppsSource, + uint8_t reserved1, + uint8_t reserved2, + uint8_t reserved3) +{ + char* pos = buffer; + + if (*size < 9 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 9; + + *pos++ = 2; + *pos++ = 55; + *pos++ = 0; + *pos++ = 0; + *pos++ = mode; + *pos++ = ppsSource; + *pos++ = reserved1; + *pos++ = reserved2; + *pos++ = reserved3; + + return E_NONE; +} + +VnError VnSpi_genWriteGpsAntennaOffset( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + vec3f position) +{ + char* pos = buffer; + + if (*size < 16 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 16; + + *pos++ = 2; + *pos++ = 57; + *pos++ = 0; + *pos++ = 0; + position.c[0] = htosf4(position.c[0]); + position.c[1] = htosf4(position.c[1]); + position.c[2] = htosf4(position.c[2]); + memcpy(pos, &position, sizeof(vec3f)); + pos += sizeof(vec3f); + + return E_NONE; +} + +VnError VnSpi_genWriteInsBasicConfiguration( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint8_t scenario, + uint8_t ahrsAiding, + uint8_t estBaseline, + uint8_t resv2) +{ + char* pos = buffer; + + if (*size < 8 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 8; + + *pos++ = 2; + *pos++ = 67; + *pos++ = 0; + *pos++ = 0; + *pos++ = scenario; + *pos++ = ahrsAiding; + *pos++ = estBaseline; + *pos++ = resv2; + + return E_NONE; +} + +VnError VnSpi_genWriteStartupFilterBiasEstimate( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + vec3f gyroBias, + vec3f accelBias, + float pressureBias) +{ + char* pos = buffer; + + if (*size < 32 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 32; + + *pos++ = 2; + *pos++ = 74; + *pos++ = 0; + *pos++ = 0; + gyroBias.c[0] = htosf4(gyroBias.c[0]); + gyroBias.c[1] = htosf4(gyroBias.c[1]); + gyroBias.c[2] = htosf4(gyroBias.c[2]); + memcpy(pos, &gyroBias, sizeof(vec3f)); + pos += sizeof(vec3f); + accelBias.c[0] = htosf4(accelBias.c[0]); + accelBias.c[1] = htosf4(accelBias.c[1]); + accelBias.c[2] = htosf4(accelBias.c[2]); + memcpy(pos, &accelBias, sizeof(vec3f)); + pos += sizeof(vec3f); + pressureBias = htosf4(pressureBias); + memcpy(pos, &pressureBias, sizeof(float)); + pos += sizeof(float); + + return E_NONE; +} + +VnError VnSpi_genWriteDeltaThetaAndDeltaVelocityConfiguration( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint8_t integrationFrame, + uint8_t gyroCompensation, + uint8_t accelCompensation, + uint8_t reserved1, + uint16_t reserved2) +{ + char* pos = buffer; + + if (*size < 10 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 10; + + *pos++ = 2; + *pos++ = 82; + *pos++ = 0; + *pos++ = 0; + *pos++ = integrationFrame; + *pos++ = gyroCompensation; + *pos++ = accelCompensation; + *pos++ = reserved1; + reserved2 = htos16(reserved2); + memcpy(pos, &reserved2, sizeof(uint16_t)); + pos += sizeof(uint16_t); + + return E_NONE; +} + +VnError VnSpi_genWriteReferenceVectorConfiguration( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint8_t useMagModel, + uint8_t useGravityModel, + uint8_t resv1, + uint8_t resv2, + uint32_t recalcThreshold, + float year, + vec3d position) +{ + char* pos = buffer; + + if (*size < 44 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 44; + + *pos++ = 2; + *pos++ = 83; + *pos++ = 0; + *pos++ = 0; + *pos++ = useMagModel; + *pos++ = useGravityModel; + *pos++ = resv1; + *pos++ = resv2; + recalcThreshold = htos32(recalcThreshold); + memcpy(pos, &recalcThreshold, sizeof(uint32_t)); + pos += sizeof(uint32_t); + year = htosf4(year); + memcpy(pos, &year, sizeof(float)); + pos += sizeof(float); + pos += 4; + position.c[0] = htosf8(position.c[0]); + position.c[1] = htosf8(position.c[1]); + position.c[2] = htosf8(position.c[2]); + memcpy(pos, &position, sizeof(vec3d)); + pos += sizeof(vec3d); + + return E_NONE; +} + +VnError VnSpi_genWriteGyroCompensation( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + mat3f c, + vec3f b) +{ + char* pos = buffer; + + if (*size < 52 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 52; + + *pos++ = 2; + *pos++ = 84; + *pos++ = 0; + *pos++ = 0; + c.e[0] = htosf4(c.e[0]); + c.e[1] = htosf4(c.e[1]); + c.e[2] = htosf4(c.e[2]); + c.e[3] = htosf4(c.e[3]); + c.e[4] = htosf4(c.e[4]); + c.e[5] = htosf4(c.e[5]); + c.e[6] = htosf4(c.e[6]); + c.e[7] = htosf4(c.e[7]); + c.e[8] = htosf4(c.e[8]); + memcpy(pos, &c, sizeof(mat3f)); + pos += sizeof(mat3f); + b.c[0] = htosf4(b.c[0]); + b.c[1] = htosf4(b.c[1]); + b.c[2] = htosf4(b.c[2]); + memcpy(pos, &b, sizeof(vec3f)); + pos += sizeof(vec3f); + + return E_NONE; +} + +VnError VnSpi_genWriteImuFilteringConfiguration( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + uint16_t magWindowSize, + uint16_t accelWindowSize, + uint16_t gyroWindowSize, + uint16_t tempWindowSize, + uint16_t presWindowSize, + uint8_t magFilterMode, + uint8_t accelFilterMode, + uint8_t gyroFilterMode, + uint8_t tempFilterMode, + uint8_t presFilterMode) +{ + char* pos = buffer; + + if (*size < 19 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 19; + + *pos++ = 2; + *pos++ = 85; + *pos++ = 0; + *pos++ = 0; + magWindowSize = htos16(magWindowSize); + memcpy(pos, &magWindowSize, sizeof(uint16_t)); + pos += sizeof(uint16_t); + accelWindowSize = htos16(accelWindowSize); + memcpy(pos, &accelWindowSize, sizeof(uint16_t)); + pos += sizeof(uint16_t); + gyroWindowSize = htos16(gyroWindowSize); + memcpy(pos, &gyroWindowSize, sizeof(uint16_t)); + pos += sizeof(uint16_t); + tempWindowSize = htos16(tempWindowSize); + memcpy(pos, &tempWindowSize, sizeof(uint16_t)); + pos += sizeof(uint16_t); + presWindowSize = htos16(presWindowSize); + memcpy(pos, &presWindowSize, sizeof(uint16_t)); + pos += sizeof(uint16_t); + *pos++ = magFilterMode; + *pos++ = accelFilterMode; + *pos++ = gyroFilterMode; + *pos++ = tempFilterMode; + *pos++ = presFilterMode; + + return E_NONE; +} + +VnError VnSpi_genWriteGpsCompassBaseline( + char* buffer, + size_t* size, + size_t desiredLength, + size_t* responseSize, + vec3f position, + vec3f uncertainty) +{ + char* pos = buffer; + + if (*size < 28 || *size < desiredLength) + return E_BUFFER_TOO_SMALL; + + *responseSize = 28; + + *pos++ = 2; + *pos++ = 93; + *pos++ = 0; + *pos++ = 0; + position.c[0] = htosf4(position.c[0]); + position.c[1] = htosf4(position.c[1]); + position.c[2] = htosf4(position.c[2]); + memcpy(pos, &position, sizeof(vec3f)); + pos += sizeof(vec3f); + uncertainty.c[0] = htosf4(uncertainty.c[0]); + uncertainty.c[1] = htosf4(uncertainty.c[1]); + uncertainty.c[2] = htosf4(uncertainty.c[2]); + memcpy(pos, &uncertainty, sizeof(vec3f)); + pos += sizeof(vec3f); + + return E_NONE; +} diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/protocol/upack.c b/src/drivers/ins/vectornav/libvnc/src/vn/protocol/upack.c new file mode 100644 index 0000000000..a99efa9d7f --- /dev/null +++ b/src/drivers/ins/vectornav/libvnc/src/vn/protocol/upack.c @@ -0,0 +1,4860 @@ +#include "vn/protocol/upack.h" + +#include +#include +#include + +#include "vn/util.h" + +/*#define MAXIMUM_REGISTER_ID 255 +#define ASCII_START_CHAR '$' +#define ASCII_END_CHAR1 '\r' +#define ASCII_END_CHAR2 '\n' +#define BINARY_START_CHAR 0xFA +#define MAX_BINARY_PACKET_SIZE 256*/ + +const uint8_t BinaryPacketGroupLengths[7][15] = { + { 8, 8, 8, 12, 16, 12, 24, 12, 12, 24, 20, 28, 2, 4, 8}, + { 8, 8, 8, 2, 8, 8, 8, 4, 4, 1, 0, 0, 0, 0, 0}, + { 2, 12, 12, 12, 4, 4, 16, 12, 12, 12, 12, 2, 40, 0, 0 }, + { 8, 8, 2, 1, 1, 24, 24, 12, 12, 12, 4, 4, 2, 28, 0 }, + { 2, 12, 16, 36, 12, 12, 12, 12, 12, 12, 28, 24, 0, 0, 0 }, + { 2, 24, 24, 12, 12, 12, 12, 12, 12, 4, 4, 68, 64, 0, 0 }, + { 8, 8, 2, 1, 1, 24, 24, 12, 12, 12, 4, 4, 2, 28, 0 }, +}; + +#define NEXT result = VnUartPacket_getNextData((uint8_t*)packet->data, &packetIndex); if (result == NULL) return; + +#define NEXTR result = VnUartPacket_getNextData((uint8_t*)packet->data, &packetIndex); if (result == NULL) return E_UNKNOWN; + +#define NEXTRAW result = (char*)VnUartPacket_getNextData((uint8_t*)packet, &packetIndex); if (result == NULL) return; + +#define ATOFF ((float)atof((char*) result)) +#define ATOFD atof((char*) result) +#define ATOU32 ((uint32_t) atoi((char*) result)) +#define ATOU16X ((uint16_t) strtol((char*) result, NULL, 16)) +#define ATOU16 ((uint16_t) atoi((char*) result)) +#define ATOU8 ((uint8_t) atoi((char*) result)) + +/* Function declarations */ +uint8_t* VnUartPacket_startAsciiPacketParse(uint8_t* packetStart, size_t* index); +uint8_t* VnUartPacket_startAsciiResponsePacketParse(uint8_t* packetStart, size_t* index); +uint8_t* VnUartPacket_getNextData(uint8_t* str, size_t* startIndex); +uint8_t* vnstrtok(uint8_t* str, size_t* startIndex); +VnError VnUartPacket_genWriteBinaryOutput( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint8_t binaryOutputNumber, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t commonField, + uint16_t timeField, + uint16_t imuField, + uint16_t gpsField, + uint16_t attitudeField, + uint16_t insField, + uint16_t gps2Field); +void VnUartPacket_startExtractionIfNeeded(VnUartPacket *packet); + +void VnUartPacket_initialize(VnUartPacket *packet, uint8_t *data, size_t length) +{ + packet->curExtractLoc = 0; + packet->length = length; + packet->data = data; +} + +void VnUartPacket_initializeFromStr(VnUartPacket* packet, char* data) +{ + VnUartPacket_initialize(packet, (uint8_t*) data, strlen(data)); +} + +bool VnUartPacket_isValid(VnUartPacket *packet) +{ + PacketType t; + + if (packet->length < 7) /* minumum binary packet is 7 bytes, minimum ASCII is 8 bytes */ + return false; + + t = VnUartPacket_type(packet); + + if (t == PACKETTYPE_ASCII) + { + /* First determine if this packet does not have a checksum or CRC. */ + if (packet->data[packet->length - 3] == 'X' && packet->data[packet->length - 4] == 'X') + return true; + + /* First determine if this packet has an 8-bit checksum or a 16-bit CRC. */ + if (packet->data[packet->length - 5] == '*') + { + /* Appears we have an 8-bit checksum packet. */ + uint8_t expectedChecksum = VnUtil_toUint8FromHexStr((char*) packet->data + packet->length - 4); + + uint8_t computedChecksum = VnChecksum8_compute((char*) packet->data + 1, packet->length - 6); + + return (bool) (expectedChecksum == computedChecksum); + } + else if (packet->data[packet->length - 7] == '*') + { + /* Appears we have a 16-bit CRC packet. */ + uint16_t packetCrc = VnUtil_toUint16FromHexStr((char*) packet->data + packet->length - 6); + + uint16_t computedCrc = VnCrc16_compute((char*) packet->data + 1, packet->length - 8); + + return (bool) (packetCrc == computedCrc); + } + else if (packet->data[packet->length - 6] == '*') /* Special Bootloader Update Response */ + { + /* Appears we have a 16-bit CRC packet. */ + uint16_t packetCrc = VnUtil_toUint16FromHexStr((char*)packet->data + packet->length - 5); + + uint16_t computedCrc = VnCrc16_compute((char*)packet->data + 1, packet->length - 7); + + return (bool)(packetCrc == computedCrc); + } + else + { + /* Don't know what we have. */ + return false; + } + } + else if (t == PACKETTYPE_BINARY) + { + uint16_t computedCrc = VnCrc16_compute((char*) packet->data + 1, packet->length - 1); + + return computedCrc == 0; + } + else + { + char bootloadSignature[] = "VectorNav Bootloader"; + if (strncmp(packet->data, bootloadSignature, sizeof(bootloadSignature) - 1) == 0) + { + return true; + } + else + { + /* Unknown packet type. */ + return false; + } + } +} + +bool VnUartPacket_isAsciiAsync(VnUartPacket *packet) +{ + /* Pointer to the unique asynchronous data type identifier. */ + char* pAT = (char*) packet->data + 3; + + if (strncmp(pAT, "YPR", 3) == 0) + return true; + if (strncmp(pAT, "QTN", 3) == 0) + return true; + #ifdef EXTRA + if (strncmp(pAT, "QTM", 3) == 0) + return true; + if (strncmp(pAT, "QTA", 3) == 0) + return true; + if (strncmp(pAT, "QTR", 3) == 0) + return true; + if (strncmp(pAT, "QMA", 3) == 0) + return true; + if (strncmp(pAT, "QAR", 3) == 0) + return true; + #endif + if (strncmp(pAT, "QMR", 3) == 0) + return true; + #ifdef EXTRA + if (strncmp(pAT, "DCM", 3) == 0) + return true; + #endif + if (strncmp(pAT, "MAG", 3) == 0) + return true; + if (strncmp(pAT, "ACC", 3) == 0) + return true; + if (strncmp(pAT, "GYR", 3) == 0) + return true; + if (strncmp(pAT, "MAR", 3) == 0) + return true; + if (strncmp(pAT, "YMR", 3) == 0) + return true; + #ifdef EXTRA + if (strncmp(pAT, "YCM", 3) == 0) + return true; + #endif + if (strncmp(pAT, "YBA", 3) == 0) + return true; + if (strncmp(pAT, "YIA", 3) == 0) + return true; + #ifdef EXTRA + if (strncmp(pAT, "ICM", 3) == 0) + return true; + #endif + if (strncmp(pAT, "IMU", 3) == 0) + return true; + if (strncmp(pAT, "GPS", 3) == 0) + return true; + if (strncmp(pAT, "GPE", 3) == 0) + return true; + if (strncmp(pAT, "INS", 3) == 0) + return true; + if (strncmp(pAT, "INE", 3) == 0) + return true; + if (strncmp(pAT, "ISL", 3) == 0) + return true; + if (strncmp(pAT, "ISE", 3) == 0) + return true; + if (strncmp(pAT, "DTV", 3) == 0) + return true; + #ifdef EXTRA + if (strncmp(pAT, "RAW", 3) == 0) + return true; + if (strncmp(pAT, "CMV", 3) == 0) + return true; + if (strncmp(pAT, "STV", 3) == 0) + return true; + if (strncmp(pAT, "COV", 3) == 0) + return true; + #endif + else + return false; +} + +bool VnUartPacket_isResponse(VnUartPacket *packet) +{ + char* p = (char*) packet->data + 3; + if (strncmp(p, "WRG", 3) == 0) + return true; + if (strncmp(p, "RRG", 3) == 0) + return true; + if (strncmp(p, "WNV", 3) == 0) + return true; + if (strncmp(p, "RFS", 3) == 0) + return true; + if (strncmp(p, "RST", 3) == 0) + return true; + if (strncmp(p, "FWU", 3) == 0) + return true; + if (strncmp(p, "CMD", 3) == 0) + return true; + if (strncmp(p, "ASY", 3) == 0) + return true; + if (strncmp(p, "TAR", 3) == 0) + return true; + if (strncmp(p, "KMD", 3) == 0) + return true; + if (strncmp(p, "KAD", 3) == 0) + return true; + if (strncmp(p, "SGB", 3) == 0) + return true; + if (strncmp(p, "DBS", 3) == 0) + return true; + if (strncmp(p, "MCU", 3) == 0) + return true; + if (strncmp(p, "SBL", 3) == 0) + return true; + if (strncmp(p, "SPS", 3) == 0) + return true; + if (strncmp(p, "BLD", 3) == 0) + return true; + if (strncmp(packet->data, "VectorNav Bootloader", 20) == 0) + return true; + + return false; +} + +bool VnUartPacket_isBootloader(VnUartPacket* packet) +{ + char* p = (char*)packet->data + 3; + if (strncmp(p, "BLD", 3) == 0) + return true; + if (strncmp(packet->data, "VectorNav Bootloader", 20) == 0) + return true; + + return false; +} + + +bool VnUartPacket_isError(VnUartPacket *packet) +{ + return VnUartPacket_isErrorRaw(packet->data); +} + +bool VnUartPacket_isErrorRaw(uint8_t *packet) +{ + return strncmp((char*) (packet + 3), "ERR", 3) == 0; +} + +PacketType VnUartPacket_type(VnUartPacket *packet) +{ + if (packet->length < 1) + /* Is really an invalid packet. */ + return PACKETTYPE_UNKNOWN; + + if (packet->data[0] == VN_ASCII_START_CHAR) + return PACKETTYPE_ASCII; + if ((uint8_t) packet->data[0] == VN_BINARY_START_CHAR) + return PACKETTYPE_BINARY; + + return PACKETTYPE_UNKNOWN; +} + +uint8_t VnUartPacket_groups(VnUartPacket* packet) +{ + return packet->data[1]; +} + +uint16_t VnUartPacket_groupField(VnUartPacket* packet, size_t groupIndex) +{ + return stoh16(*((uint16_t*) (packet->data + groupIndex * sizeof(uint16_t) + 2))); +} + +VnAsciiAsync VnUartPacket_determineAsciiAsyncType(VnUartPacket *packet) +{ + /* Pointer to the unique asynchronous data type identifier. */ + char* pAT = (char*) (packet->data + 3); + + if (strncmp(pAT, "YPR", 3) == 0) + return VNYPR; + if (strncmp(pAT, "QTN", 3) == 0) + return VNQTN; + #ifdef EXTRA + if (strncmp(pAT, "QTM", 3) == 0) + return VNQTM; + if (strncmp(pAT, "QTA", 3) == 0) + return VNQTA; + if (strncmp(pAT, "QTR", 3) == 0) + return VNQTR; + if (strncmp(pAT, "QMA", 3) == 0) + return VNQMA; + if (strncmp(pAT, "QAR", 3) == 0) + return VNQAR; + #endif + if (strncmp(pAT, "QMR", 3) == 0) + return VNQMR; + #ifdef EXTRA + if (strncmp(pAT, "DCM", 3) == 0) + return VNDCM; + #endif + if (strncmp(pAT, "MAG", 3) == 0) + return VNMAG; + if (strncmp(pAT, "ACC", 3) == 0) + return VNACC; + if (strncmp(pAT, "GYR", 3) == 0) + return VNGYR; + if (strncmp(pAT, "MAR", 3) == 0) + return VNMAR; + if (strncmp(pAT, "YMR", 3) == 0) + return VNYMR; + #ifdef EXTRA + if (strncmp(pAT, "YCM", 3) == 0) + return VNYCM; + #endif + if (strncmp(pAT, "YBA", 3) == 0) + return VNYBA; + if (strncmp(pAT, "YIA", 3) == 0) + return VNYIA; + #ifdef EXTRA + if (strncmp(pAT, "ICM", 3) == 0) + return VNICM; + #endif + if (strncmp(pAT, "IMU", 3) == 0) + return VNIMU; + if (strncmp(pAT, "GPS", 3) == 0) + return VNGPS; + if (strncmp(pAT, "GPE", 3) == 0) + return VNGPE; + if (strncmp(pAT, "INS", 3) == 0) + return VNINS; + if (strncmp(pAT, "INE", 3) == 0) + return VNINE; + if (strncmp(pAT, "ISL", 3) == 0) + return VNISL; + if (strncmp(pAT, "ISE", 3) == 0) + return VNISE; + if (strncmp(pAT, "DTV", 3) == 0) + return VNDTV; + if (strncmp(pAT, "RTK", 3) == 0) + return VNRTK; + #ifdef EXTRA + if (strncmp(pAT, "RAW", 3) == 0) + return VNRAW; + if (strncmp(pAT, "CMV", 3) == 0) + return VNCMV; + if (strncmp(pAT, "STV", 3) == 0) + return VNSTV; + if (strncmp(pAT, "COV", 3) == 0) + return VNCOV; + #endif + else + /* Can't determine the packet type. */ + return VNOFF; +} + +size_t VnUartPacket_computeNumOfBytesForBinaryGroupPayload(BinaryGroupType groupType, uint16_t groupField) +{ + size_t runningLength = 0; + size_t i; + + /* Determine which group is present. */ + size_t groupIndex = 0; + for (i = 0; i < 7; i++, groupIndex++) + { + if (((size_t) groupType >> i) & 0x01) + break; + } + + for (i = 0; i < sizeof(uint16_t) * 8; i++) + { + if ((groupField >> i) & 1) + { + runningLength += BinaryPacketGroupLengths[groupIndex][i]; + } + } + + return runningLength; +} + +bool VnUartPacket_isCompatible( + VnUartPacket *packet, + CommonGroup commonGroup, + TimeGroup timeGroup, + ImuGroup imuGroup, + GpsGroup gpsGroup, + AttitudeGroup attitudeGroup, + InsGroup insGroup, + GpsGroup gps2Group) +{ + /* First make sure the appropriate groups are specified. */ + uint8_t groups = packet->data[1]; + uint8_t *curField = packet->data + 2; + + if (commonGroup) + { + if (stoh16(*(uint16_t*) curField) != commonGroup) + /* Not the expected collection of field data types. */ + return false; + + curField += 2; + } + else if (groups & 0x01) + { + /* There is unexpected Common Group data. */ + return false; + } + + if (timeGroup) + { + if (stoh16(*(uint16_t*) curField) != timeGroup) + /* Not the expected collection of field data types. */ + return false; + + curField += 2; + } + else if (groups & 0x02) + { + /* There is unexpected Time Group data. */ + return false; + } + + if (imuGroup) + { + if (stoh16(*(uint16_t*) curField) != imuGroup) + /* Not the expected collection of field data types. */ + return false; + + curField += 2; + } + else if (groups & 0x04) + { + /* There is unexpected IMU Group data. */ + return false; + } + + if (gpsGroup) + { + if (stoh16(*(uint16_t*) curField) != gpsGroup) + /* Not the expected collection of field data types. */ + return false; + + curField += 2; + } + else if (groups & 0x08) + { + /* There is unexpected GPS Group data. */ + return false; + } + + if (attitudeGroup) + { + if (stoh16(*(uint16_t*) curField) != attitudeGroup) + /* Not the expected collection of field data types. */ + return false; + + curField += 2; + } + else if (groups & 0x10) + { + /* There is unexpected Attitude Group data. */ + return false; + } + + if (insGroup) + { + if (stoh16(*(uint16_t*) curField) != insGroup) + /* Not the expected collection of field data types. */ + return false; + + curField += 2; + } + else if (groups & 0x20) + { + /* There is unexpected INS Group data. */ + return false; + } + + if (gps2Group) + { + if (stoh16(*(uint16_t*)curField) != gps2Group) + /* Not the expected collection of field data types. */ + return false; + + curField += 2; + } + else if (groups & 0x40) + { + /* There is unexpected GPS2 Group data. */ + return false; + } + + /* Everything checks out. */ + return true; +} + +void VnUartPacket_startExtractionIfNeeded(VnUartPacket *packet) +{ + if (packet->curExtractLoc == 0) + /* Determine the location to start extracting. */ + packet->curExtractLoc = VnUtil_countSetBitsUint8(packet->data[1]) * 2 + 2; +} + +uint8_t VnUartPacket_extractUint8(VnUartPacket *packet) +{ + uint8_t d; + + VnUartPacket_startExtractionIfNeeded(packet); + + d = *(uint8_t*) (packet->data + packet->curExtractLoc); + + packet->curExtractLoc += sizeof(uint8_t); + + return d; +} + +int8_t VnUartPacket_extractInt8(VnUartPacket *packet) +{ + int8_t d; + + VnUartPacket_startExtractionIfNeeded(packet); + + d = *(int8_t*) (packet->data + packet->curExtractLoc); + + packet->curExtractLoc += sizeof(int8_t); + + return d; +} + +uint16_t VnUartPacket_extractUint16(VnUartPacket *packet) +{ + uint8_t* extractLocation; + + VnUartPacket_startExtractionIfNeeded(packet); + + extractLocation = packet->data + packet->curExtractLoc; + + packet->curExtractLoc += sizeof(uint16_t); + + return VnUtil_extractUint16((char*) extractLocation); +} + +uint32_t VnUartPacket_extractUint32(VnUartPacket *packet) +{ + uint8_t* extractLocation; + + VnUartPacket_startExtractionIfNeeded(packet); + + extractLocation = packet->data + packet->curExtractLoc; + + packet->curExtractLoc += sizeof(uint32_t); + + return VnUtil_extractUint32((char*) extractLocation); +} + +uint64_t VnUartPacket_extractUint64(VnUartPacket *packet) +{ + uint64_t d; + + VnUartPacket_startExtractionIfNeeded(packet); + + memcpy(&d, packet->data + packet->curExtractLoc, sizeof(uint64_t)); + + packet->curExtractLoc += sizeof(uint64_t); + + return stoh64(d); +} + +float VnUartPacket_extractFloat(VnUartPacket* packet) +{ + uint8_t* extractLocation; + + VnUartPacket_startExtractionIfNeeded(packet); + + extractLocation = packet->data + packet->curExtractLoc; + + packet->curExtractLoc += sizeof(float); + + return VnUtil_extractFloat((char*) extractLocation); +} + +vec3f VnUartPacket_extractVec3f(VnUartPacket *packet) +{ + uint8_t* extractLocation; + + VnUartPacket_startExtractionIfNeeded(packet); + + extractLocation = packet->data + packet->curExtractLoc; + + packet->curExtractLoc += 3 * sizeof(float); + + return VnUtil_extractVec3f((char*) extractLocation); +} + +vec3d VnUartPacket_extractVec3d(VnUartPacket *packet) +{ + uint8_t* extractLocation; + + VnUartPacket_startExtractionIfNeeded(packet); + + extractLocation = packet->data + packet->curExtractLoc; + + packet->curExtractLoc += 3 * sizeof(double); + + return VnUtil_extractVec3d((char*) extractLocation); +} + +vec4f VnUartPacket_extractVec4f(VnUartPacket *packet) +{ + uint8_t* extractLocation; + + VnUartPacket_startExtractionIfNeeded(packet); + + extractLocation = packet->data + packet->curExtractLoc; + + packet->curExtractLoc += 4 * sizeof(float); + + return VnUtil_extractVec4f((char*) extractLocation); +} + +mat3f VnUartPacket_extractMat3f(VnUartPacket *packet) +{ + uint8_t* extractLocation; + + VnUartPacket_startExtractionIfNeeded(packet); + + extractLocation = packet->data + packet->curExtractLoc; + + packet->curExtractLoc += 9 * sizeof(float); + + return VnUtil_extractMat3f((char*) extractLocation); +} + +GpsDop VnUartPacket_extractGpsDop(VnUartPacket *packet) +{ + uint8_t* extractLocation; + + VnUartPacket_startExtractionIfNeeded(packet); + + extractLocation = packet->data + packet->curExtractLoc; + + packet->curExtractLoc += sizeof(GpsDop); + + return VnUtil_extractGpsDop((char*)extractLocation); +} + +TimeUtc VnUartPacket_extractTimeUtc(VnUartPacket *packet) +{ + uint8_t* extractLocation; + + VnUartPacket_startExtractionIfNeeded(packet); + + extractLocation = packet->data + packet->curExtractLoc; + + packet->curExtractLoc += sizeof(TimeUtc); + + return VnUtil_extractTimeUtc((char*)extractLocation); +} + +TimeInfo VnUartPacket_extractTimeInfo(VnUartPacket *packet) +{ + uint8_t* extractLocation; + + VnUartPacket_startExtractionIfNeeded(packet); + + extractLocation = packet->data + packet->curExtractLoc; + + packet->curExtractLoc += sizeof(TimeInfo); + + return VnUtil_extractTimeInfo((char*)extractLocation); +} + +size_t VnUartPacket_computeBinaryPacketLength(uint8_t const* startOfPossibleBinaryPacket) +{ + uint8_t groupsPresent = startOfPossibleBinaryPacket[1]; + size_t runningPayloadLength = 2; /* Start of packet character plus groups present field. */ + uint8_t const* pCurrentGroupField = startOfPossibleBinaryPacket + 2; + + if (groupsPresent & 0x01) + { + runningPayloadLength += 2 + VnUartPacket_computeNumOfBytesForBinaryGroupPayload(BINARYGROUPTYPE_COMMON, stoh16(*(uint16_t*)pCurrentGroupField)); + pCurrentGroupField += 2; + } + + if (groupsPresent & 0x02) + { + runningPayloadLength += 2 + VnUartPacket_computeNumOfBytesForBinaryGroupPayload(BINARYGROUPTYPE_TIME, stoh16(*(uint16_t*)pCurrentGroupField)); + pCurrentGroupField += 2; + } + + if (groupsPresent & 0x04) + { + runningPayloadLength += 2 + VnUartPacket_computeNumOfBytesForBinaryGroupPayload(BINARYGROUPTYPE_IMU, stoh16(*(uint16_t*)pCurrentGroupField)); + pCurrentGroupField += 2; + } + + if (groupsPresent & 0x08) + { + runningPayloadLength += 2 + VnUartPacket_computeNumOfBytesForBinaryGroupPayload(BINARYGROUPTYPE_GPS, stoh16(*(uint16_t*)pCurrentGroupField)); + pCurrentGroupField += 2; + } + + if (groupsPresent & 0x10) + { + runningPayloadLength += 2 + VnUartPacket_computeNumOfBytesForBinaryGroupPayload(BINARYGROUPTYPE_ATTITUDE, stoh16(*(uint16_t*)pCurrentGroupField)); + pCurrentGroupField += 2; + } + + if (groupsPresent & 0x20) + { + runningPayloadLength += 2 + VnUartPacket_computeNumOfBytesForBinaryGroupPayload(BINARYGROUPTYPE_INS, stoh16(*(uint16_t*)pCurrentGroupField)); + pCurrentGroupField += 2; + } + + if (groupsPresent & 0x40) + { + runningPayloadLength += 2 + VnUartPacket_computeNumOfBytesForBinaryGroupPayload(BINARYGROUPTYPE_GPS2, stoh16(*(uint16_t*)pCurrentGroupField)); + pCurrentGroupField += 2; + } + + return runningPayloadLength + 2; /* Add 2 bytes for CRC. */ +} + +uint8_t* VnUartPacket_startAsciiPacketParse(uint8_t* packetStart, size_t* index) +{ + *index = 7; + + return vnstrtok(packetStart, index); +} + +uint8_t* VnUartPacket_startAsciiResponsePacketParse(uint8_t* packetStart, size_t* index) +{ + VnUartPacket_startAsciiPacketParse(packetStart, index); + + return vnstrtok(packetStart, index); +} + +uint8_t* VnUartPacket_getNextData(uint8_t* str, size_t* startIndex) +{ + return vnstrtok(str, startIndex); +} + +uint8_t* vnstrtok(uint8_t* str, size_t* startIndex) +{ + size_t origIndex = *startIndex; + + while (str[*startIndex] != ',' && str[*startIndex] != '*') + (*startIndex)++; + + str[(*startIndex)++] = '\0'; + + return str + origIndex; +} + +void VnUartPacket_parseVNYPR(VnUartPacket* packet, vec3f* yawPitchRoll) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + yawPitchRoll->c[0] = ATOFF; NEXT + yawPitchRoll->c[1] = ATOFF; NEXT + yawPitchRoll->c[2] = ATOFF; +} + +void VnUartPacket_parseVNQTN(VnUartPacket* packet, vec4f* quaternion) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + quaternion->c[0] = ATOFF; NEXT + quaternion->c[1] = ATOFF; NEXT + quaternion->c[2] = ATOFF; NEXT + quaternion->c[3] = ATOFF; +} + +#ifdef EXTRA + +void VnUartPacket_parseVNQTM(VnUartPacket* packet, vec4f *quaternion, vec3f *magnetic) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + quaternion->c[0] = ATOFF; NEXT + quaternion->c[1] = ATOFF; NEXT + quaternion->c[2] = ATOFF; NEXT + quaternion->c[3] = ATOFF; NEXT + magnetic->c[0] = ATOFF; NEXT + magnetic->c[1] = ATOFF; NEXT + magnetic->c[2] = ATOFF; +} + +void VnUartPacket_parseVNQTA(VnUartPacket* packet, vec4f* quaternion, vec3f* acceleration) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + quaternion->c[0] = ATOFF; NEXT + quaternion->c[1] = ATOFF; NEXT + quaternion->c[2] = ATOFF; NEXT + quaternion->c[3] = ATOFF; NEXT + acceleration->c[0] = ATOFF; NEXT + acceleration->c[1] = ATOFF; NEXT + acceleration->c[2] = ATOFF; +} + +void VnUartPacket_parseVNQTR(VnUartPacket* packet, vec4f* quaternion, vec3f* angularRate) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + quaternion->c[0] = ATOFF; NEXT + quaternion->c[1] = ATOFF; NEXT + quaternion->c[2] = ATOFF; NEXT + quaternion->c[3] = ATOFF; NEXT + angularRate->c[0] = ATOFF; NEXT + angularRate->c[1] = ATOFF; NEXT + angularRate->c[2] = ATOFF; +} + +void VnUartPacket_parseVNQMA(VnUartPacket* packet, vec4f* quaternion, vec3f* magnetic, vec3f* acceleration) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + quaternion->c[0] = ATOFF; NEXT + quaternion->c[1] = ATOFF; NEXT + quaternion->c[2] = ATOFF; NEXT + quaternion->c[3] = ATOFF; NEXT + magnetic->c[0] = ATOFF; NEXT + magnetic->c[1] = ATOFF; NEXT + magnetic->c[2] = ATOFF; NEXT + acceleration->c[0] = ATOFF; NEXT + acceleration->c[1] = ATOFF; NEXT + acceleration->c[2] = ATOFF; +} + +void VnUartPacket_parseVNQAR(VnUartPacket* packet, vec4f* quaternion, vec3f* acceleration, vec3f* angularRate) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + quaternion->c[0] = ATOFF; NEXT + quaternion->c[1] = ATOFF; NEXT + quaternion->c[2] = ATOFF; NEXT + quaternion->c[3] = ATOFF; NEXT + acceleration->c[0] = ATOFF; NEXT + acceleration->c[1] = ATOFF; NEXT + acceleration->c[2] = ATOFF; NEXT + angularRate->c[0] = ATOFF; NEXT + angularRate->c[1] = ATOFF; NEXT + angularRate->c[2] = ATOFF; +} + +#endif + +void VnUartPacket_parseVNQMR(VnUartPacket* packet, vec4f* quaternion, vec3f* magnetic, vec3f* acceleration, vec3f* angularRate) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + quaternion->c[0] = ATOFF; NEXT + quaternion->c[1] = ATOFF; NEXT + quaternion->c[2] = ATOFF; NEXT + quaternion->c[3] = ATOFF; NEXT + magnetic->c[0] = ATOFF; NEXT + magnetic->c[1] = ATOFF; NEXT + magnetic->c[2] = ATOFF; NEXT + acceleration->c[0] = ATOFF; NEXT + acceleration->c[1] = ATOFF; NEXT + acceleration->c[2] = ATOFF; NEXT + angularRate->c[0] = ATOFF; NEXT + angularRate->c[1] = ATOFF; NEXT + angularRate->c[2] = ATOFF; +} + +#ifdef EXTRA + +void VnUartPacket_parseVNDCM(VnUartPacket* packet, mat3f* dcm) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + dcm->c[0] = ATOFF; NEXT + dcm->c[3] = ATOFF; NEXT + dcm->c[6] = ATOFF; NEXT + dcm->c[1] = ATOFF; NEXT + dcm->c[4] = ATOFF; NEXT + dcm->c[7] = ATOFF; NEXT + dcm->c[2] = ATOFF; NEXT + dcm->c[5] = ATOFF; NEXT + dcm->c[8] = ATOFF; +} + +#endif + +void VnUartPacket_parseVNMAG(VnUartPacket* packet, vec3f* magnetic) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + magnetic->c[0] = ATOFF; NEXT + magnetic->c[1] = ATOFF; NEXT + magnetic->c[2] = ATOFF; +} + +void VnUartPacket_parseVNACC(VnUartPacket* packet, vec3f* acceleration) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + acceleration->c[0] = ATOFF; NEXT + acceleration->c[1] = ATOFF; NEXT + acceleration->c[2] = ATOFF; +} + +void VnUartPacket_parseVNGYR(VnUartPacket* packet, vec3f* angularRate) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + angularRate->c[0] = ATOFF; NEXT + angularRate->c[1] = ATOFF; NEXT + angularRate->c[2] = ATOFF; +} + +void VnUartPacket_parseVNMAR(VnUartPacket* packet, vec3f* magnetic, vec3f* acceleration, vec3f* angularRate) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + magnetic->c[0] = ATOFF; NEXT + magnetic->c[1] = ATOFF; NEXT + magnetic->c[2] = ATOFF; NEXT + acceleration->c[0] = ATOFF; NEXT + acceleration->c[1] = ATOFF; NEXT + acceleration->c[2] = ATOFF; NEXT + angularRate->c[0] = ATOFF; NEXT + angularRate->c[1] = ATOFF; NEXT + angularRate->c[2] = ATOFF; +} + +VnError VnUartPacket_parseVNYMR(VnUartPacket* packet, vec3f* yawPitchRoll, vec3f* magnetic, vec3f* acceleration, vec3f* angularRate) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + yawPitchRoll->c[0] = ATOFF; NEXTR + yawPitchRoll->c[1] = ATOFF; NEXTR + yawPitchRoll->c[2] = ATOFF; NEXTR + magnetic->c[0] = ATOFF; NEXTR + magnetic->c[1] = ATOFF; NEXTR + magnetic->c[2] = ATOFF; NEXTR + acceleration->c[0] = ATOFF; NEXTR + acceleration->c[1] = ATOFF; NEXTR + acceleration->c[2] = ATOFF; NEXTR + angularRate->c[0] = ATOFF; NEXTR + angularRate->c[1] = ATOFF; NEXTR + angularRate->c[2] = ATOFF; + + return E_NONE; +} + +/*VnError vn_uart_packet_parse_vnymr_buffer( + uint8_t* packetBuf, + size_t packetLen, + vec3f *yawPitchRoll, + vec3f *magnetic, + vec3f *acceleration, + vec3f *angularRate) +{ + VnUartPacket p; + + vn_uart_packet_init(&p, packetBuf, packetLen); + + return vn_uart_packet_parse_vnymr(&p, yawPitchRoll, magnetic, acceleration, angularRate); +}*/ + +#ifdef EXTRA + +void VnUartPacket_parseVNYCM(VnUartPacket* packet, vec3f* yawPitchRoll, vec3f* magnetic, vec3f* acceleration, vec3f* angularRate, float* temperature) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + yawPitchRoll->c[0] = ATOFF; NEXT + yawPitchRoll->c[1] = ATOFF; NEXT + yawPitchRoll->c[2] = ATOFF; NEXT + magnetic->c[0] = ATOFF; NEXT + magnetic->c[1] = ATOFF; NEXT + magnetic->c[2] = ATOFF; NEXT + acceleration->c[0] = ATOFF; NEXT + acceleration->c[1] = ATOFF; NEXT + acceleration->c[2] = ATOFF; NEXT + angularRate->c[0] = ATOFF; NEXT + angularRate->c[1] = ATOFF; NEXT + angularRate->c[2] = ATOFF; NEXT + *temperature = ATOFF; +} + +#endif + +void VnUartPacket_parseVNYBA(VnUartPacket* packet, vec3f* yawPitchRoll, vec3f* accelerationBody, vec3f* angularRate) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + yawPitchRoll->c[0] = ATOFF; NEXT + yawPitchRoll->c[1] = ATOFF; NEXT + yawPitchRoll->c[2] = ATOFF; NEXT + accelerationBody->c[0] = ATOFF; NEXT + accelerationBody->c[1] = ATOFF; NEXT + accelerationBody->c[2] = ATOFF; NEXT + angularRate->c[0] = ATOFF; NEXT + angularRate->c[1] = ATOFF; NEXT + angularRate->c[2] = ATOFF; +} + +void VnUartPacket_parseVNYIA(VnUartPacket* packet, vec3f* yawPitchRoll, vec3f* accelerationInertial, vec3f* angularRate) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + yawPitchRoll->c[0] = ATOFF; NEXT + yawPitchRoll->c[1] = ATOFF; NEXT + yawPitchRoll->c[2] = ATOFF; NEXT + accelerationInertial->c[0] = ATOFF; NEXT + accelerationInertial->c[1] = ATOFF; NEXT + accelerationInertial->c[2] = ATOFF; NEXT + angularRate->c[0] = ATOFF; NEXT + angularRate->c[1] = ATOFF; NEXT + angularRate->c[2] = ATOFF; +} + +#ifdef EXTRA + +void VnUartPacket_parseVNICM(VnUartPacket* packet, vec3f* yawPitchRoll, vec3f* magnetic, vec3f* accelerationInertial, vec3f* angularRate) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + yawPitchRoll->c[0] = ATOFF; NEXT + yawPitchRoll->c[1] = ATOFF; NEXT + yawPitchRoll->c[2] = ATOFF; NEXT + magnetic->c[0] = ATOFF; NEXT + magnetic->c[1] = ATOFF; NEXT + magnetic->c[2] = ATOFF; NEXT + accelerationInertial->c[0] = ATOFF; NEXT + accelerationInertial->c[1] = ATOFF; NEXT + accelerationInertial->c[2] = ATOFF; NEXT + angularRate->c[0] = ATOFF; NEXT + angularRate->c[1] = ATOFF; NEXT + angularRate->c[2] = ATOFF; +} + +#endif + +void VnUartPacket_parseVNIMU(VnUartPacket* packet, vec3f* magneticUncompensated, vec3f* accelerationUncompensated, vec3f* angularRateUncompensated, float* temperature, float* pressure) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + magneticUncompensated->c[0] = ATOFF; NEXT + magneticUncompensated->c[1] = ATOFF; NEXT + magneticUncompensated->c[2] = ATOFF; NEXT + accelerationUncompensated->c[0] = ATOFF; NEXT + accelerationUncompensated->c[1] = ATOFF; NEXT + accelerationUncompensated->c[2] = ATOFF; NEXT + angularRateUncompensated->c[0] = ATOFF; NEXT + angularRateUncompensated->c[1] = ATOFF; NEXT + angularRateUncompensated->c[2] = ATOFF; NEXT + *temperature = ATOFF; NEXT + *pressure = ATOFF; +} + +void VnUartPacket_parseVNGPS(VnUartPacket* packet, double* time, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, vec3d* lla, vec3f* nedVel, vec3f* nedAcc, float* speedAcc, float* timeAcc) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + *time = ATOFD; NEXT + *week = ATOU16; NEXT + *gpsFix = ATOU8; NEXT + *numSats = ATOU8; NEXT + lla->c[0] = ATOFD; NEXT + lla->c[1] = ATOFD; NEXT + lla->c[2] = ATOFD; NEXT + nedVel->c[0] = ATOFF; NEXT + nedVel->c[1] = ATOFF; NEXT + nedVel->c[2] = ATOFF; NEXT + nedAcc->c[0] = ATOFF; NEXT + nedAcc->c[1] = ATOFF; NEXT + nedAcc->c[2] = ATOFF; NEXT + *speedAcc = ATOFF; NEXT + *timeAcc = ATOFF; +} + +void VnUartPacket_parseVNINS(VnUartPacket* packet, double* time, uint16_t* week, uint16_t* status, vec3f* yawPitchRoll, vec3d* lla, vec3f* nedVel, float* attUncertainty, float* posUncertainty, float* velUncertainty) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + *time = ATOFD; NEXT + *week = ATOU16; NEXT + *status = ATOU16X; NEXT + yawPitchRoll->c[0] = ATOFF; NEXT + yawPitchRoll->c[1] = ATOFF; NEXT + yawPitchRoll->c[2] = ATOFF; NEXT + lla->c[0] = ATOFD; NEXT + lla->c[1] = ATOFD; NEXT + lla->c[2] = ATOFD; NEXT + nedVel->c[0] = ATOFF; NEXT + nedVel->c[1] = ATOFF; NEXT + nedVel->c[2] = ATOFF; NEXT + *attUncertainty = ATOFF; NEXT + *posUncertainty = ATOFF; NEXT + *velUncertainty = ATOFF; +} + +void VnUartPacket_parseVNINE(VnUartPacket* packet, double* time, uint16_t* week, uint16_t* status, vec3f* yawPitchRoll, vec3d* position, vec3f* velocity, float* attUncertainty, float* posUncertainty, float* velUncertainty) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + *time = ATOFD; NEXT + *week = ATOU16; NEXT + *status = ATOU16X; NEXT + yawPitchRoll->c[0] = ATOFF; NEXT + yawPitchRoll->c[1] = ATOFF; NEXT + yawPitchRoll->c[2] = ATOFF; NEXT + position->c[0] = ATOFD; NEXT + position->c[1] = ATOFD; NEXT + position->c[2] = ATOFD; NEXT + velocity->c[0] = ATOFF; NEXT + velocity->c[1] = ATOFF; NEXT + velocity->c[2] = ATOFF; NEXT + *attUncertainty = ATOFF; NEXT + *posUncertainty = ATOFF; NEXT + *velUncertainty = ATOFF; +} + +void VnUartPacket_parseVNISL(VnUartPacket* packet, vec3f* ypr, vec3d* lla, vec3f* velocity, vec3f* acceleration, vec3f* angularRate) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + ypr->c[0] = ATOFF; NEXT + ypr->c[1] = ATOFF; NEXT + ypr->c[2] = ATOFF; NEXT + lla->c[0] = ATOFD; NEXT + lla->c[1] = ATOFD; NEXT + lla->c[2] = ATOFD; NEXT + velocity->c[0] = ATOFF; NEXT + velocity->c[1] = ATOFF; NEXT + velocity->c[2] = ATOFF; NEXT + acceleration->c[0] = ATOFF; NEXT + acceleration->c[1] = ATOFF; NEXT + acceleration->c[2] = ATOFF; NEXT + angularRate->c[0] = ATOFF; NEXT + angularRate->c[1] = ATOFF; NEXT + angularRate->c[2] = ATOFF; +} + +void VnUartPacket_parseVNISE(VnUartPacket* packet, vec3f* ypr, vec3d* position, vec3f* velocity, vec3f* acceleration, vec3f* angularRate) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + ypr->c[0] = ATOFF; NEXT + ypr->c[1] = ATOFF; NEXT + ypr->c[2] = ATOFF; NEXT + position->c[0] = ATOFD; NEXT + position->c[1] = ATOFD; NEXT + position->c[2] = ATOFD; NEXT + velocity->c[0] = ATOFF; NEXT + velocity->c[1] = ATOFF; NEXT + velocity->c[2] = ATOFF; NEXT + acceleration->c[0] = ATOFF; NEXT + acceleration->c[1] = ATOFF; NEXT + acceleration->c[2] = ATOFF; NEXT + angularRate->c[0] = ATOFF; NEXT + angularRate->c[1] = ATOFF; NEXT + angularRate->c[2] = ATOFF; +} + +#ifdef EXTRA + +void VnUartPacket_parseVNRAW(VnUartPacket* packet, vec3f *magneticVoltage, vec3f *accelerationVoltage, vec3f *angularRateVoltage, float* temperatureVoltage) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + magneticVoltage->c[0] = ATOFF; NEXT + magneticVoltage->c[1] = ATOFF; NEXT + magneticVoltage->c[2] = ATOFF; NEXT + accelerationVoltage->c[0] = ATOFF; NEXT + accelerationVoltage->c[1] = ATOFF; NEXT + accelerationVoltage->c[2] = ATOFF; NEXT + angularRateVoltage->c[0] = ATOFF; NEXT + angularRateVoltage->c[1] = ATOFF; NEXT + angularRateVoltage->c[2] = ATOFF; NEXT + *temperatureVoltage = ATOFF; +} + +void VnUartPacket_parseVNCMV(VnUartPacket* packet, vec3f* magneticUncompensated, vec3f* accelerationUncompensated, vec3f* angularRateUncompensated, float* temperature) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + magneticUncompensated->c[0] = ATOFF; NEXT + magneticUncompensated->c[1] = ATOFF; NEXT + magneticUncompensated->c[2] = ATOFF; NEXT + accelerationUncompensated->c[0] = ATOFF; NEXT + accelerationUncompensated->c[1] = ATOFF; NEXT + accelerationUncompensated->c[2] = ATOFF; NEXT + angularRateUncompensated->c[0] = ATOFF; NEXT + angularRateUncompensated->c[1] = ATOFF; NEXT + angularRateUncompensated->c[2] = ATOFF; NEXT + *temperature = ATOFF; +} + +void VnUartPacket_parseVNSTV(VnUartPacket* packet, vec4f* quaternion, vec3f* angularRateBias) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + quaternion->c[0] = ATOFF; NEXT + quaternion->c[1] = ATOFF; NEXT + quaternion->c[2] = ATOFF; NEXT + quaternion->c[3] = ATOFF; NEXT + angularRateBias->c[0] = ATOFF; NEXT + angularRateBias->c[1] = ATOFF; NEXT + angularRateBias->c[2] = ATOFF; +} + +void VnUartPacket_parseVNCOV(VnUartPacket* packet, vec3f* attitudeVariance, vec3f* angularRateBiasVariance) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + attitudeVariance->c[0] = ATOFF; NEXT + attitudeVariance->c[1] = ATOFF; NEXT + attitudeVariance->c[2] = ATOFF; NEXT + angularRateBiasVariance->c[0] = ATOFF; NEXT + angularRateBiasVariance->c[1] = ATOFF; NEXT + angularRateBiasVariance->c[2] = ATOFF; +} + +#endif + +void VnUartPacket_parseVNGPE(VnUartPacket* packet, double* tow, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, vec3d* position, vec3f* velocity, vec3f* posAcc, float* speedAcc, float* timeAcc) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + *tow = ATOFD; NEXT + *week = ATOU16; NEXT + *gpsFix = ATOU8; NEXT + *numSats = ATOU8; NEXT + position->c[0] = ATOFD; NEXT + position->c[1] = ATOFD; NEXT + position->c[2] = ATOFD; NEXT + velocity->c[0] = ATOFF; NEXT + velocity->c[1] = ATOFF; NEXT + velocity->c[2] = ATOFF; NEXT + posAcc->c[0] = ATOFF; NEXT + posAcc->c[1] = ATOFF; NEXT + posAcc->c[2] = ATOFF; NEXT + *speedAcc = ATOFF; NEXT + *timeAcc = ATOFF; +} + +void VnUartPacket_parseVNDTV(VnUartPacket* packet, float* deltaTime, vec3f* deltaTheta, vec3f* deltaVelocity) +{ + size_t packetIndex; + + uint8_t* result = VnUartPacket_startAsciiPacketParse(packet->data, &packetIndex); + + *deltaTime = ATOFF; NEXT + deltaTheta->c[0] = ATOFF; NEXT + deltaTheta->c[1] = ATOFF; NEXT + deltaTheta->c[2] = ATOFF; NEXT + deltaVelocity->c[0] = ATOFF; NEXT + deltaVelocity->c[1] = ATOFF; NEXT + deltaVelocity->c[2] = ATOFF; +} + +VnError VnUartPacket_genWrite( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + uint16_t registerId, + size_t *cmdSize, + char const *format, + ...) +{ + va_list ap; + uint8_t *curOutputLoc = buffer; + char const *curFormatPos = format; + + /* Avoid a compiler warning by doing this */ + (void)bufferSize; + + #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 + + /* Add the message header and register number. */ + curOutputLoc += sprintf((char*) curOutputLoc, "$VNWRG,%d", registerId); + + va_start(ap, format); + + /* Now cycle through the provided format specifier. */ + while (*curFormatPos != '\0') + { + switch (*curFormatPos++) + { + case 'U': + + switch (*curFormatPos++) + { + case '1': + /* 'uint8_t' is promoted to 'int' when passed through '...'. */ + curOutputLoc += sprintf((char*) curOutputLoc, ",%d", va_arg(ap, int)); + break; + + case '2': + /* 'uint16_t' is promoted to 'int' when passed through '...'. */ + curOutputLoc += sprintf((char*) curOutputLoc, ",%d", va_arg(ap, int)); + break; + + case '4': + curOutputLoc += sprintf((char*) curOutputLoc, ",%d", va_arg(ap, uint32_t)); + break; + } + + break; + + case 'F': + + switch (*curFormatPos++) + { + case '4': + /* 'float' is promoted to 'double' when passed through '...'. */ + curOutputLoc += sprintf((char*) curOutputLoc, ",%f", va_arg(ap, double)); + break; + + case '8': + curOutputLoc += sprintf((char*) curOutputLoc, ",%f", va_arg(ap, double)); + break; + } + + break; + + case 'S': + curOutputLoc += sprintf((char*) curOutputLoc,",%s",va_arg(ap,char *)); + break; + } + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif + + va_end(ap); + + /* Compute and append the checksum. */ + *curOutputLoc++ = '*'; + + if (errorDetectionMode == VNERRORDETECTIONMODE_NONE) + { + *curOutputLoc++ = 'X'; + *curOutputLoc++ = 'X'; + } + else if (errorDetectionMode == VNERRORDETECTIONMODE_CHECKSUM) + { + uint8_t checksum = VnChecksum8_compute((char*) (buffer + 1), curOutputLoc - buffer - 2); + VnUtil_toHexStrFromUint8(checksum, (char*) curOutputLoc); + + curOutputLoc += 2; + } + else if (errorDetectionMode == VNERRORDETECTIONMODE_CRC) + { + uint16_t crc = VnCrc16_compute((char*) (buffer + 1), curOutputLoc - buffer - 2); + VnUtil_toHexStrFromUint16(crc, (char*) curOutputLoc); + + curOutputLoc += 4; + } + else + { + return E_NOT_SUPPORTED; + } + + *curOutputLoc++ = '\r'; + *curOutputLoc++ = '\n'; + + *cmdSize = curOutputLoc - buffer; + + return E_NONE; +} + +VnError VnUartPacket_genReadBinaryOutput1( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s((char*)buffer, bufferSize, "$VNRRG,75"); + #else + *cmdSize = sprintf((char*) buffer, "$VNRRG,75"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, buffer, cmdSize); +} + +VnError VnUartPacket_genReadBinaryOutput2( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s((char*)buffer, bufferSize, "$VNRRG,76"); + #else + *cmdSize = sprintf((char*) buffer, "$VNRRG,76"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, buffer, cmdSize); +} + +VnError VnUartPacket_genReadBinaryOutput3( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s((char*)buffer, bufferSize, "$VNRRG,77"); + #else + *cmdSize = sprintf((char*) buffer, "$VNRRG,77"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, buffer, cmdSize); +} + +#ifdef EXTRA + +VnError VnUartPacket_genReadBinaryOutput4( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,78"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,78"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, buffer, cmdSize); +} + +VnError VnUartPacket_genReadBinaryOutput5( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,79"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,79"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, buffer, cmdSize); +} + +#endif + +VnError VnUartPacket_finalizeCommand(VnErrorDetectionMode errorDetectionMode, uint8_t *packet, size_t *length) +{ + #if defined(_MSC_VER) + /* Disable warnings regarding using sprintf_s. */ + #pragma warning(push) + #pragma warning(disable:4996) + #endif + + if (errorDetectionMode == VNERRORDETECTIONMODE_CHECKSUM) + { + *length += sprintf((char*) (packet + *length), "*%02X\r\n", VnChecksum8_compute((char*) packet + 1, *length - 1)); + } + else if (errorDetectionMode == VNERRORDETECTIONMODE_CRC) + { + *length += sprintf((char*) (packet + *length), "*%04X\r\n", VnCrc16_compute((char*) packet + 1, *length - 1)); + } + else + { + *length += sprintf((char*) (packet + *length), "*XX\r\n"); + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif + + return E_NONE; +} + +VnError VnUartPacket_genCmdWriteSettings( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s((char*)buffer, bufferSize, "$VNWNV"); + #else + *cmdSize = sprintf((char*) buffer, "$VNWNV"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, buffer, cmdSize); +} + +VnError VnUartPacket_genCmdRestoreFactorySettings( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s((char*)buffer, bufferSize, "$VNRFS"); + #else + *cmdSize = sprintf((char*) buffer, "$VNRFS"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, buffer, cmdSize); +} + +VnError VnUartPacket_genCmdReset( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s((char*)buffer, bufferSize, "$VNRST"); + #else + *cmdSize = sprintf((char*) buffer, "$VNRST"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, buffer, cmdSize); +} + +VnError VnUartPacket_genCmdFirmwareUpdate( + uint8_t* buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t* cmdSize) +{ +#if VN_HAVE_SECURE_CRT + * cmdSize = sprintf_s((char*)buffer, bufferSize, "$VNFWU"); +#else + * cmdSize = sprintf((char*)buffer, "$VNFWU"); +#endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, buffer, cmdSize); +} + + +VnError VnUartPacket_genCmdTare( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s((char*)buffer, bufferSize, "$VNTAR"); + #else + *cmdSize = sprintf((char*) buffer, "$VNTAR"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, buffer, cmdSize); +} + +VnError VnUartPacket_genCmdSetGyroBias( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s((char*)buffer, bufferSize, "$VNSGB"); + #else + *cmdSize = sprintf((char*) buffer, "$VNSGB"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, buffer, cmdSize); +} + +VnError VnUartPacket_genCmdKnownMagneticDisturbance( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + bool disturbancePresent, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s((char*)buffer, bufferSize, "$VNKMD,%d", disturbancePresent ? 1 : 0); + #else + *cmdSize = sprintf((char*) buffer, "$VNKMD,%d", disturbancePresent ? 1 : 0); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, buffer, cmdSize); +} + +VnError VnUartPacket_genCmdKnownAccelerationDisturbance( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + bool disturbancePresent, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s((char*)buffer, bufferSize, "$VNKAD,%d", disturbancePresent ? 1 : 0); + #else + *cmdSize = sprintf((char*) buffer, "$VNKAD,%d", disturbancePresent ? 1 : 0); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, buffer, cmdSize); +} + +VnError VnUartPacket_genReadUserTag( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,00"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,00"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadModelNumber( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,01"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,01"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadHardwareRevision( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,02"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,02"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadSerialNumber( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,03"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,03"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadFirmwareVersion( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,04"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,04"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadSerialBaudRate( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,05"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,05"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadAsyncDataOutputType( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,06"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,06"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadAsyncDataOutputFrequency( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,07"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,07"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadYawPitchRoll( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,08"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,08"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadAttitudeQuaternion( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,09"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,09"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadQuaternionMagneticAccelerationAndAngularRates( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,15"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,15"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadMagneticMeasurements( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,17"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,17"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadAccelerationMeasurements( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,18"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,18"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadAngularRateMeasurements( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,19"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,19"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadMagneticAccelerationAndAngularRates( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,20"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,20"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadMagneticAndGravityReferenceVectors( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,21"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,21"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadMagnetometerCompensation( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,23"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,23"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadAccelerationCompensation( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,25"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,25"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadReferenceFrameRotation( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,26"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,26"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadYawPitchRollMagneticAccelerationAndAngularRates( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,27"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,27"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadCommunicationProtocolControl( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,30"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,30"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadSynchronizationControl( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,32"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,32"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadSynchronizationStatus( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,33"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,33"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadVpeBasicControl( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,35"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,35"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadVpeMagnetometerBasicTuning( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,36"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,36"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadVpeAccelerometerBasicTuning( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,38"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,38"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadMagnetometerCalibrationControl( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,44"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,44"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadCalculatedMagnetometerCalibration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,47"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,47"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadVelocityCompensationMeasurement( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,50"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,50"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadVelocityCompensationControl( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,51"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,51"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadImuMeasurements( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,54"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,54"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadGpsConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,55"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,55"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadGpsAntennaOffset( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,57"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,57"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadGpsSolutionLla( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,58"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,58"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadGpsSolutionEcef( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,59"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,59"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadInsSolutionLla( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,63"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,63"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadInsSolutionEcef( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,64"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,64"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadInsBasicConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,67"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,67"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadInsStateLla( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,72"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,72"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadInsStateEcef( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,73"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,73"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadStartupFilterBiasEstimate( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,74"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,74"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadDeltaThetaAndDeltaVelocity( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,80"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,80"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadDeltaThetaAndDeltaVelocityConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,82"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,82"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadReferenceVectorConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,83"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,83"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadGyroCompensation( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,84"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,84"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadImuFilteringConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,85"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,85"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadGpsCompassBaseline( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,93"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,93"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadGpsCompassEstimatedBaseline( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,97"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,97"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadYawPitchRollTrueBodyAccelerationAndAngularRates( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,239"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,239"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genReadYawPitchRollTrueInertialAccelerationAndAngularRates( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize) +{ + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s(buffer, bufferSize, "$VNRRG,240"); + #else + *cmdSize = sprintf(buffer, "$VNRRG,240"); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, (uint8_t*)buffer, cmdSize); +} + +VnError VnUartPacket_genWriteBinaryOutput( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint8_t binaryOutputNumber, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t commonField, + uint16_t timeField, + uint16_t imuField, + uint16_t gpsField, + uint16_t attitudeField, + uint16_t insField, + uint16_t gps2Field) +{ + uint16_t groups = 0; + #if !VN_HAVE_SECURE_CRT + char* cbp = (char*)buffer; + #endif + + /* First determine which groups are present. */ + if (commonField) + groups |= 0x0001; + if (timeField) + groups |= 0x0002; + if (imuField) + groups |= 0x0004; + if (gpsField) + groups |= 0x0008; + if (attitudeField) + groups |= 0x0010; + if (insField) + groups |= 0x0020; + if (gps2Field) + groups |= 0x0040; + + #if VN_HAVE_SECURE_CRT + *cmdSize = sprintf_s((char*)buffer, bufferSize, "$VNWRG,%u,%u,%u,%X", 74 + binaryOutputNumber, asyncMode, rateDivisor, groups); + #else + *cmdSize = sprintf(cbp, "$VNWRG,%u,%u,%u,%X", 74 + binaryOutputNumber, asyncMode, rateDivisor, groups); + #endif + + if (commonField) + #if VN_HAVE_SECURE_CRT + *cmdSize += sprintf_s((char*)(buffer + *cmdSize), bufferSize - *cmdSize, ",%X", commonField); + #else + *cmdSize += sprintf(cbp + *cmdSize, ",%X", commonField); + #endif + if (timeField) + #if VN_HAVE_SECURE_CRT + *cmdSize += sprintf_s((char*)(buffer + *cmdSize), bufferSize - *cmdSize, ",%X", timeField); + #else + *cmdSize += sprintf(cbp + *cmdSize, ",%X", timeField); + #endif + if (imuField) + #if VN_HAVE_SECURE_CRT + *cmdSize += sprintf_s((char*)(buffer + *cmdSize), bufferSize - *cmdSize, ",%X", imuField); + #else + *cmdSize += sprintf(cbp + *cmdSize, ",%X", imuField); + #endif + if (gpsField) + #if VN_HAVE_SECURE_CRT + *cmdSize += sprintf_s((char*)(buffer + *cmdSize), bufferSize - *cmdSize, ",%X", gpsField); + #else + *cmdSize += sprintf(cbp + *cmdSize, ",%X", gpsField); + #endif + if (attitudeField) + #if VN_HAVE_SECURE_CRT + *cmdSize += sprintf_s((char*)(buffer + *cmdSize), bufferSize - *cmdSize, ",%X", attitudeField); + #else + *cmdSize += sprintf(cbp + *cmdSize, ",%X", attitudeField); + #endif + if (insField) + #if VN_HAVE_SECURE_CRT + *cmdSize += sprintf_s((char*)(buffer + *cmdSize), bufferSize - *cmdSize, ",%X", insField); + #else + *cmdSize += sprintf(cbp + *cmdSize, ",%X", insField); + #endif + if (gps2Field) + #if VN_HAVE_SECURE_CRT + *cmdSize += sprintf_s((char*)(buffer + *cmdSize), bufferSize - *cmdSize, ",%X", gps2Field); + #else + *cmdSize += sprintf(cbp + *cmdSize, ",%X", gps2Field); + #endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, buffer, cmdSize); +} + +VnError VnUartPacket_genWriteBinaryOutput1( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t commonField, + uint16_t timeField, + uint16_t imuField, + uint16_t gpsField, + uint16_t attitudeField, + uint16_t insField, + uint16_t gps2Field) +{ + return VnUartPacket_genWriteBinaryOutput( + buffer, + bufferSize, + errorDetectionMode, + cmdSize, + 1, + asyncMode, + rateDivisor, + commonField, + timeField, + imuField, + gpsField, + attitudeField, + insField, + gps2Field); +} + +VnError VnUartPacket_genWriteBinaryOutput2( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t commonField, + uint16_t timeField, + uint16_t imuField, + uint16_t gpsField, + uint16_t attitudeField, + uint16_t insField, + uint16_t gps2Field) +{ + return VnUartPacket_genWriteBinaryOutput( + buffer, + bufferSize, + errorDetectionMode, + cmdSize, + 2, + asyncMode, + rateDivisor, + commonField, + timeField, + imuField, + gpsField, + attitudeField, + insField, + gps2Field); +} + +VnError VnUartPacket_genWriteBinaryOutput3( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t commonField, + uint16_t timeField, + uint16_t imuField, + uint16_t gpsField, + uint16_t attitudeField, + uint16_t insField, + uint16_t gps2Field) +{ + return VnUartPacket_genWriteBinaryOutput( + buffer, + bufferSize, + errorDetectionMode, + cmdSize, + 3, + asyncMode, + rateDivisor, + commonField, + timeField, + imuField, + gpsField, + attitudeField, + insField, + gps2Field); +} + +#ifdef EXTRA + +VnError VnUartPacket_genWriteBinaryOutput4( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t commonField, + uint16_t timeField, + uint16_t imuField, + uint16_t gpsField, + uint16_t attitudeField, + uint16_t insField, + uint16_t gps2Field) +{ + return VnUartPacket_genWriteBinaryOutput( + buffer, + bufferSize, + errorDetectionMode, + cmdSize, + 4, + asyncMode, + rateDivisor, + commonField, + timeField, + imuField, + gpsField, + attitudeField, + insField, + gps2Field); +} + +VnError VnUartPacket_genWriteBinaryOutput5( + uint8_t *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t commonField, + uint16_t timeField, + uint16_t imuField, + uint16_t gpsField, + uint16_t attitudeField, + uint16_t insField, + uint16_t gps2Field) +{ + return VnUartPacket_genWriteBinaryOutput( + buffer, + bufferSize, + errorDetectionMode, + cmdSize, + 5, + asyncMode, + rateDivisor, + commonField, + timeField, + imuField, + gpsField, + attitudeField, + insField, + gps2Field); +} + +#endif + +VnError VnUartPacket_genWriteFirmwareUpdate( + char* buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t* cmdSize, + char* record) +{ +#if VN_HAVE_SECURE_CRT + * cmdSize = sprintf_s((char*)buffer, bufferSize, "$VNBLD,%s",record); +#else + * cmdSize = sprintf((char*)buffer, "$VNBLD,%s", record); +#endif + + return VnUartPacket_finalizeCommand(errorDetectionMode, buffer, cmdSize); +} + +VnError VnUartPacket_genWriteUserTag( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + char* tag) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 0, + cmdSize, + "S", + tag); +} + +VnError VnUartPacket_genWriteSerialBaudRate( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint32_t baudrate) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 5, + cmdSize, + "U4", + baudrate); +} + +VnError VnUartPacket_genWriteSerialBaudRate_with_options( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint32_t baudrate) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 5, + cmdSize, + "U4", + baudrate); +} + +VnError VnUartPacket_genWriteAsyncDataOutputType( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint32_t ador) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 6, + cmdSize, + "U4", + ador); +} + +VnError VnUartPacket_genWriteAsyncDataOutputType_with_options( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint32_t ador) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 6, + cmdSize, + "U4", + ador); +} + +VnError VnUartPacket_genWriteAsyncDataOutputFrequency( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint32_t adof) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 7, + cmdSize, + "U4", + adof); +} + +VnError VnUartPacket_genWriteAsyncDataOutputFrequency_with_options( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint32_t adof) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 7, + cmdSize, + "U4", + adof); +} + +VnError VnUartPacket_genWriteMagneticAndGravityReferenceVectors( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + vec3f magRef, + vec3f accRef) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 21, + cmdSize, + "F4F4F4F4F4F4", + magRef.c[0], + magRef.c[1], + magRef.c[2], + accRef.c[0], + accRef.c[1], + accRef.c[2]); +} + +VnError VnUartPacket_genWriteMagnetometerCompensation( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + mat3f c, + vec3f b) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 23, + cmdSize, + "F4F4F4F4F4F4F4F4F4F4F4F4", + c.e[0], + c.e[3], + c.e[6], + c.e[1], + c.e[4], + c.e[7], + c.e[2], + c.e[5], + c.e[8], + b.c[0], + b.c[1], + b.c[2]); +} + +VnError VnUartPacket_genWriteAccelerationCompensation( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + mat3f c, + vec3f b) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 25, + cmdSize, + "F4F4F4F4F4F4F4F4F4F4F4F4", + c.e[0], + c.e[3], + c.e[6], + c.e[1], + c.e[4], + c.e[7], + c.e[2], + c.e[5], + c.e[8], + b.c[0], + b.c[1], + b.c[2]); +} + +VnError VnUartPacket_genWriteReferenceFrameRotation( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + mat3f c) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 26, + cmdSize, + "F4F4F4F4F4F4F4F4F4", + c.e[0], + c.e[3], + c.e[6], + c.e[1], + c.e[4], + c.e[7], + c.e[2], + c.e[5], + c.e[8]); +} + +VnError VnUartPacket_genWriteCommunicationProtocolControl( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint8_t serialCount, + uint8_t serialStatus, + uint8_t spiCount, + uint8_t spiStatus, + uint8_t serialChecksum, + uint8_t spiChecksum, + uint8_t errorMode) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 30, + cmdSize, + "U1U1U1U1U1U1U1", + serialCount, + serialStatus, + spiCount, + spiStatus, + serialChecksum, + spiChecksum, + errorMode); +} + +VnError VnUartPacket_genWriteSynchronizationControl( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint8_t syncInMode, + uint8_t syncInEdge, + uint16_t syncInSkipFactor, + uint32_t reserved1, + uint8_t syncOutMode, + uint8_t syncOutPolarity, + uint16_t syncOutSkipFactor, + uint32_t syncOutPulseWidth, + uint32_t reserved2) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 32, + cmdSize, + "U1U1U2U4U1U1U2U4U4", + syncInMode, + syncInEdge, + syncInSkipFactor, + reserved1, + syncOutMode, + syncOutPolarity, + syncOutSkipFactor, + syncOutPulseWidth, + reserved2); +} + +VnError VnUartPacket_genWriteSynchronizationStatus( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint32_t syncInCount, + uint32_t syncInTime, + uint32_t syncOutCount) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 33, + cmdSize, + "U4U4U4", + syncInCount, + syncInTime, + syncOutCount); +} + +VnError VnUartPacket_genWriteVpeBasicControl( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint8_t enable, + uint8_t headingMode, + uint8_t filteringMode, + uint8_t tuningMode) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 35, + cmdSize, + "U1U1U1U1", + enable, + headingMode, + filteringMode, + tuningMode); +} + +VnError VnUartPacket_genWriteVpeMagnetometerBasicTuning( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + vec3f baseTuning, + vec3f adaptiveTuning, + vec3f adaptiveFiltering) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 36, + cmdSize, + "F4F4F4F4F4F4F4F4F4", + baseTuning.c[0], + baseTuning.c[1], + baseTuning.c[2], + adaptiveTuning.c[0], + adaptiveTuning.c[1], + adaptiveTuning.c[2], + adaptiveFiltering.c[0], + adaptiveFiltering.c[1], + adaptiveFiltering.c[2]); +} + +VnError VnUartPacket_genWriteVpeAccelerometerBasicTuning( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + vec3f baseTuning, + vec3f adaptiveTuning, + vec3f adaptiveFiltering) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 38, + cmdSize, + "F4F4F4F4F4F4F4F4F4", + baseTuning.c[0], + baseTuning.c[1], + baseTuning.c[2], + adaptiveTuning.c[0], + adaptiveTuning.c[1], + adaptiveTuning.c[2], + adaptiveFiltering.c[0], + adaptiveFiltering.c[1], + adaptiveFiltering.c[2]); +} + +VnError VnUartPacket_genWriteMagnetometerCalibrationControl( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint8_t hsiMode, + uint8_t hsiOutput, + uint8_t convergeRate) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 44, + cmdSize, + "U1U1U1", + hsiMode, + hsiOutput, + convergeRate); +} + +VnError VnUartPacket_genWriteVelocityCompensationMeasurement( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + vec3f velocity) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 50, + cmdSize, + "F4F4F4", + velocity.c[0], + velocity.c[1], + velocity.c[2]); +} + +VnError VnUartPacket_genWriteVelocityCompensationControl( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint8_t mode, + float velocityTuning, + float rateTuning) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 51, + cmdSize, + "U1F4F4", + mode, + velocityTuning, + rateTuning); +} + +VnError VnUartPacket_genWriteGpsConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint8_t mode, + uint8_t ppsSource, + uint8_t reserved1, + uint8_t reserved2, + uint8_t reserved3) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 55, + cmdSize, + "U1U1U1U1U1", + mode, + ppsSource, + reserved1, + reserved2, + reserved3); +} + +VnError VnUartPacket_genWriteGpsAntennaOffset( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + vec3f position) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 57, + cmdSize, + "F4F4F4", + position.c[0], + position.c[1], + position.c[2]); +} + +VnError VnUartPacket_genWriteInsBasicConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint8_t scenario, + uint8_t ahrsAiding, + uint8_t estBaseline, + uint8_t resv2) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 67, + cmdSize, + "U1U1U1U1", + scenario, + ahrsAiding, + estBaseline, + resv2); +} + +VnError VnUartPacket_genWriteStartupFilterBiasEstimate( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + vec3f gyroBias, + vec3f accelBias, + float pressureBias) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 74, + cmdSize, + "F4F4F4F4F4F4F4", + gyroBias.c[0], + gyroBias.c[1], + gyroBias.c[2], + accelBias.c[0], + accelBias.c[1], + accelBias.c[2], + pressureBias); +} + +VnError VnUartPacket_genWriteDeltaThetaAndDeltaVelocityConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint8_t integrationFrame, + uint8_t gyroCompensation, + uint8_t accelCompensation, + uint8_t reserved1, + uint16_t reserved2) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 82, + cmdSize, + "U1U1U1U1U2", + integrationFrame, + gyroCompensation, + accelCompensation, + reserved1, + reserved2); +} + +VnError VnUartPacket_genWriteReferenceVectorConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint8_t useMagModel, + uint8_t useGravityModel, + uint8_t resv1, + uint8_t resv2, + uint32_t recalcThreshold, + float year, + vec3d position) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 83, + cmdSize, + "U1U1U1U1U4F4F8F8F8", + useMagModel, + useGravityModel, + resv1, + resv2, + recalcThreshold, + year, + position.c[0], + position.c[1], + position.c[2]); +} + +VnError VnUartPacket_genWriteGyroCompensation( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + mat3f c, + vec3f b) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 84, + cmdSize, + "F4F4F4F4F4F4F4F4F4F4F4F4", + c.e[0], + c.e[3], + c.e[6], + c.e[1], + c.e[4], + c.e[7], + c.e[2], + c.e[5], + c.e[8], + b.c[0], + b.c[1], + b.c[2]); +} + +VnError VnUartPacket_genWriteImuFilteringConfiguration( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + uint16_t magWindowSize, + uint16_t accelWindowSize, + uint16_t gyroWindowSize, + uint16_t tempWindowSize, + uint16_t presWindowSize, + uint8_t magFilterMode, + uint8_t accelFilterMode, + uint8_t gyroFilterMode, + uint8_t tempFilterMode, + uint8_t presFilterMode) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 85, + cmdSize, + "U2U2U2U2U2U1U1U1U1U1", + magWindowSize, + accelWindowSize, + gyroWindowSize, + tempWindowSize, + presWindowSize, + magFilterMode, + accelFilterMode, + gyroFilterMode, + tempFilterMode, + presFilterMode); +} + +VnError VnUartPacket_genWriteGpsCompassBaseline( + char *buffer, + size_t bufferSize, + VnErrorDetectionMode errorDetectionMode, + size_t *cmdSize, + vec3f position, + vec3f uncertainty) +{ + return VnUartPacket_genWrite( + (uint8_t*)buffer, + bufferSize, + errorDetectionMode, + 93, + cmdSize, + "F4F4F4F4F4F4", + position.c[0], + position.c[1], + position.c[2], + uncertainty.c[0], + uncertainty.c[1], + uncertainty.c[2]); +} + +void VnUartPacket_parseError(VnUartPacket *packet, uint8_t *error) +{ + VnUartPacket_parseErrorRaw(packet->data, error); +} + +void VnUartPacket_parseErrorRaw(uint8_t *packet, uint8_t *error) +{ + *error = (uint8_t) strtol((char*)(packet + 7),NULL,16); +} + +void VnUartPacket_parseBinaryOutput( + VnUartPacket *packet, + uint16_t* asyncMode, + uint16_t* rateDivisor, + uint16_t* outputGroup, + uint16_t* commonField, + uint16_t* timeField, + uint16_t* imuField, + uint16_t* gpsField, + uint16_t* attitudeField, + uint16_t* insField, + uint16_t* gps2Field) +{ + VnUartPacket_parseBinaryOutputRaw( + packet->data, + asyncMode, + rateDivisor, + outputGroup, + commonField, + timeField, + imuField, + gpsField, + attitudeField, + insField, + gps2Field); +} + +void VnUartPacket_parseBinaryOutputRaw( + uint8_t *packet, + uint16_t* asyncMode, + uint16_t* rateDivisor, + uint16_t* outputGroup, + uint16_t* commonField, + uint16_t* timeField, + uint16_t* imuField, + uint16_t* gpsField, + uint16_t* attitudeField, + uint16_t* insField, + uint16_t* gps2Field) +{ + size_t packetIndex; + + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse(packet, &packetIndex); + + *commonField = 0; + *timeField = 0; + *imuField = 0; + *gpsField = 0; + *attitudeField = 0; + *insField = 0; + *gps2Field = 0; + + *asyncMode = ATOU16; + NEXTRAW + *rateDivisor = ATOU16; + NEXTRAW + *outputGroup = ATOU16X; + if (*outputGroup & 0x0001) + { + NEXTRAW + *commonField = ATOU16X; + } + if (*outputGroup & 0x0002) + { + NEXTRAW + *timeField = ATOU16X; + } + if (*outputGroup & 0x0004) + { + NEXTRAW + *imuField = ATOU16X; + } + if (*outputGroup & 0x0008) + { + NEXTRAW + *gpsField = ATOU16X; + } + if (*outputGroup & 0x0010) + { + NEXTRAW + *attitudeField = ATOU16X; + } + if (*outputGroup & 0x0020) + { + NEXTRAW + *insField = ATOU16X; + } + if (*outputGroup & 0x0040) + { + NEXTRAW + *gps2Field = ATOU16X; + } +} + +void VnUartPacket_parseUserTag(VnUartPacket *packet, char *tag) +{ + VnUartPacket_parseUserTagRaw((char*)packet->data, tag); +} + +void VnUartPacket_parseUserTagRaw(char *packet, char* tag) +{ + size_t packetIndex; + char* next; + + next = (char*)VnUartPacket_startAsciiPacketParse((uint8_t*)packet, &packetIndex); + + if (*(next + strlen(next) + 1) == '*') + { + tag[0] = '\0'; + return; + } + + next = (char*)VnUartPacket_getNextData((uint8_t*)packet, &packetIndex); + + #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 + strcpy(tag, next); + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void VnUartPacket_parseModelNumber(VnUartPacket *packet, char *productName) +{ + VnUartPacket_parseModelNumberRaw((char*)packet->data, productName); +} + +void VnUartPacket_parseModelNumberRaw(char *packet, char* productName) +{ + size_t packetIndex; + char* next; + + next = (char*)VnUartPacket_startAsciiPacketParse((uint8_t*)packet, &packetIndex); + + if (*(next + strlen(next) + 1) == '*') + { + productName[0] = '\0'; + return; + } + + next = (char*)VnUartPacket_getNextData((uint8_t*)packet, &packetIndex); + + #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 + strcpy(productName, next); + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void VnUartPacket_parseHardwareRevision(VnUartPacket *packet, uint32_t* revision) +{ + VnUartPacket_parseHardwareRevisionRaw((char*)packet->data, revision); +} + +void VnUartPacket_parseHardwareRevisionRaw(char *packet, uint32_t* revision) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *revision = ATOU32; +} + +void VnUartPacket_parseSerialNumber(VnUartPacket *packet, uint32_t* serialNum) +{ + VnUartPacket_parseSerialNumberRaw((char*)packet->data, serialNum); +} + +void VnUartPacket_parseSerialNumberRaw(char *packet, uint32_t* serialNum) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *serialNum = ATOU32; +} + +void VnUartPacket_parseFirmwareVersion(VnUartPacket *packet, char *firmwareVersion) +{ + VnUartPacket_parseFirmwareVersionRaw((char*)packet->data, firmwareVersion); +} + +void VnUartPacket_parseFirmwareVersionRaw(char *packet, char* firmwareVersion) +{ + size_t packetIndex; + char* next; + + next = (char*)VnUartPacket_startAsciiPacketParse((uint8_t*)packet, &packetIndex); + + if (*(next + strlen(next) + 1) == '*') + { + firmwareVersion[0] = '\0'; + return; + } + + next = (char*)VnUartPacket_getNextData((uint8_t*)packet, &packetIndex); + + #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 + strcpy(firmwareVersion, next); + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void VnUartPacket_parseSerialBaudRate(VnUartPacket *packet, uint32_t* baudrate) +{ + VnUartPacket_parseSerialBaudRateRaw((char*)packet->data, baudrate); +} + +void VnUartPacket_parseSerialBaudRateRaw(char *packet, uint32_t* baudrate) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *baudrate = ATOU32; +} + +void VnUartPacket_parseAsyncDataOutputType(VnUartPacket *packet, uint32_t* ador) +{ + VnUartPacket_parseAsyncDataOutputTypeRaw((char*)packet->data, ador); +} + +void VnUartPacket_parseAsyncDataOutputTypeRaw(char *packet, uint32_t* ador) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *ador = ATOU32; +} + +void VnUartPacket_parseAsyncDataOutputFrequency(VnUartPacket *packet, uint32_t* adof) +{ + VnUartPacket_parseAsyncDataOutputFrequencyRaw((char*)packet->data, adof); +} + +void VnUartPacket_parseAsyncDataOutputFrequencyRaw(char *packet, uint32_t* adof) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *adof = ATOU32; +} + +void VnUartPacket_parseYawPitchRoll(VnUartPacket *packet, vec3f* yawPitchRoll) +{ + VnUartPacket_parseYawPitchRollRaw((char*)packet->data, yawPitchRoll); +} + +void VnUartPacket_parseYawPitchRollRaw(char *packet, vec3f* yawPitchRoll) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + yawPitchRoll->c[0] = ATOFF; NEXTRAW + yawPitchRoll->c[1] = ATOFF; NEXTRAW + yawPitchRoll->c[2] = ATOFF; +} + +void VnUartPacket_parseAttitudeQuaternion(VnUartPacket *packet, vec4f* quat) +{ + VnUartPacket_parseAttitudeQuaternionRaw((char*)packet->data, quat); +} + +void VnUartPacket_parseAttitudeQuaternionRaw(char *packet, vec4f* quat) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + quat->c[0] = ATOFF; NEXTRAW + quat->c[1] = ATOFF; NEXTRAW + quat->c[2] = ATOFF; NEXTRAW + quat->c[3] = ATOFF; +} + +void VnUartPacket_parseQuaternionMagneticAccelerationAndAngularRates(VnUartPacket *packet, vec4f* quat, vec3f* mag, vec3f* accel, vec3f* gyro) +{ + VnUartPacket_parseQuaternionMagneticAccelerationAndAngularRatesRaw((char*)packet->data, quat, mag, accel, gyro); +} + +void VnUartPacket_parseQuaternionMagneticAccelerationAndAngularRatesRaw(char *packet, vec4f* quat, vec3f* mag, vec3f* accel, vec3f* gyro) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + quat->c[0] = ATOFF; NEXTRAW + quat->c[1] = ATOFF; NEXTRAW + quat->c[2] = ATOFF; NEXTRAW + quat->c[3] = ATOFF; NEXTRAW + mag->c[0] = ATOFF; NEXTRAW + mag->c[1] = ATOFF; NEXTRAW + mag->c[2] = ATOFF; NEXTRAW + accel->c[0] = ATOFF; NEXTRAW + accel->c[1] = ATOFF; NEXTRAW + accel->c[2] = ATOFF; NEXTRAW + gyro->c[0] = ATOFF; NEXTRAW + gyro->c[1] = ATOFF; NEXTRAW + gyro->c[2] = ATOFF; +} + +void VnUartPacket_parseMagneticMeasurements(VnUartPacket *packet, vec3f* mag) +{ + VnUartPacket_parseMagneticMeasurementsRaw((char*)packet->data, mag); +} + +void VnUartPacket_parseMagneticMeasurementsRaw(char *packet, vec3f* mag) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + mag->c[0] = ATOFF; NEXTRAW + mag->c[1] = ATOFF; NEXTRAW + mag->c[2] = ATOFF; +} + +void VnUartPacket_parseAccelerationMeasurements(VnUartPacket *packet, vec3f* accel) +{ + VnUartPacket_parseAccelerationMeasurementsRaw((char*)packet->data, accel); +} + +void VnUartPacket_parseAccelerationMeasurementsRaw(char *packet, vec3f* accel) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + accel->c[0] = ATOFF; NEXTRAW + accel->c[1] = ATOFF; NEXTRAW + accel->c[2] = ATOFF; +} + +void VnUartPacket_parseAngularRateMeasurements(VnUartPacket *packet, vec3f* gyro) +{ + VnUartPacket_parseAngularRateMeasurementsRaw((char*)packet->data, gyro); +} + +void VnUartPacket_parseAngularRateMeasurementsRaw(char *packet, vec3f* gyro) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + gyro->c[0] = ATOFF; NEXTRAW + gyro->c[1] = ATOFF; NEXTRAW + gyro->c[2] = ATOFF; +} + +void VnUartPacket_parseMagneticAccelerationAndAngularRates(VnUartPacket *packet, vec3f* mag, vec3f* accel, vec3f* gyro) +{ + VnUartPacket_parseMagneticAccelerationAndAngularRatesRaw((char*)packet->data, mag, accel, gyro); +} + +void VnUartPacket_parseMagneticAccelerationAndAngularRatesRaw(char *packet, vec3f* mag, vec3f* accel, vec3f* gyro) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + mag->c[0] = ATOFF; NEXTRAW + mag->c[1] = ATOFF; NEXTRAW + mag->c[2] = ATOFF; NEXTRAW + accel->c[0] = ATOFF; NEXTRAW + accel->c[1] = ATOFF; NEXTRAW + accel->c[2] = ATOFF; NEXTRAW + gyro->c[0] = ATOFF; NEXTRAW + gyro->c[1] = ATOFF; NEXTRAW + gyro->c[2] = ATOFF; +} + +void VnUartPacket_parseMagneticAndGravityReferenceVectors(VnUartPacket *packet, vec3f* magRef, vec3f* accRef) +{ + VnUartPacket_parseMagneticAndGravityReferenceVectorsRaw((char*)packet->data, magRef, accRef); +} + +void VnUartPacket_parseMagneticAndGravityReferenceVectorsRaw(char *packet, vec3f* magRef, vec3f* accRef) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + magRef->c[0] = ATOFF; NEXTRAW + magRef->c[1] = ATOFF; NEXTRAW + magRef->c[2] = ATOFF; NEXTRAW + accRef->c[0] = ATOFF; NEXTRAW + accRef->c[1] = ATOFF; NEXTRAW + accRef->c[2] = ATOFF; +} + +void VnUartPacket_parseFilterMeasurementsVarianceParameters(VnUartPacket *packet, float* angularWalkVariance, vec3f* angularRateVariance, vec3f* magneticVariance, vec3f* accelerationVariance) +{ + VnUartPacket_parseFilterMeasurementsVarianceParametersRaw((char*)packet->data, angularWalkVariance, angularRateVariance, magneticVariance, accelerationVariance); +} + +void VnUartPacket_parseFilterMeasurementsVarianceParametersRaw(char *packet, float* angularWalkVariance, vec3f* angularRateVariance, vec3f* magneticVariance, vec3f* accelerationVariance) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *angularWalkVariance = ATOFF; NEXTRAW + angularRateVariance->c[0] = ATOFF; NEXTRAW + angularRateVariance->c[1] = ATOFF; NEXTRAW + angularRateVariance->c[2] = ATOFF; NEXTRAW + magneticVariance->c[0] = ATOFF; NEXTRAW + magneticVariance->c[1] = ATOFF; NEXTRAW + magneticVariance->c[2] = ATOFF; NEXTRAW + accelerationVariance->c[0] = ATOFF; NEXTRAW + accelerationVariance->c[1] = ATOFF; NEXTRAW + accelerationVariance->c[2] = ATOFF; +} + +void VnUartPacket_parseMagnetometerCompensation(VnUartPacket *packet, mat3f* c, vec3f* b) +{ + VnUartPacket_parseMagnetometerCompensationRaw((char*)packet->data, c, b); +} + +void VnUartPacket_parseMagnetometerCompensationRaw(char *packet, mat3f* c, vec3f* b) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + c->e[0] = ATOFF; NEXTRAW + c->e[3] = ATOFF; NEXTRAW + c->e[6] = ATOFF; NEXTRAW + c->e[1] = ATOFF; NEXTRAW + c->e[4] = ATOFF; NEXTRAW + c->e[7] = ATOFF; NEXTRAW + c->e[2] = ATOFF; NEXTRAW + c->e[5] = ATOFF; NEXTRAW + c->e[8] = ATOFF; NEXTRAW + b->c[0] = ATOFF; NEXTRAW + b->c[1] = ATOFF; NEXTRAW + b->c[2] = ATOFF; +} + +void VnUartPacket_parseFilterActiveTuningParameters(VnUartPacket *packet, float* magneticDisturbanceGain, float* accelerationDisturbanceGain, float* magneticDisturbanceMemory, float* accelerationDisturbanceMemory) +{ + VnUartPacket_parseFilterActiveTuningParametersRaw((char*)packet->data, magneticDisturbanceGain, accelerationDisturbanceGain, magneticDisturbanceMemory, accelerationDisturbanceMemory); +} + +void VnUartPacket_parseFilterActiveTuningParametersRaw(char *packet, float* magneticDisturbanceGain, float* accelerationDisturbanceGain, float* magneticDisturbanceMemory, float* accelerationDisturbanceMemory) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *magneticDisturbanceGain = ATOFF; NEXTRAW + *accelerationDisturbanceGain = ATOFF; NEXTRAW + *magneticDisturbanceMemory = ATOFF; NEXTRAW + *accelerationDisturbanceMemory = ATOFF; +} + +void VnUartPacket_parseAccelerationCompensation(VnUartPacket *packet, mat3f* c, vec3f* b) +{ + VnUartPacket_parseAccelerationCompensationRaw((char*)packet->data, c, b); +} + +void VnUartPacket_parseAccelerationCompensationRaw(char *packet, mat3f* c, vec3f* b) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + c->e[0] = ATOFF; NEXTRAW + c->e[3] = ATOFF; NEXTRAW + c->e[6] = ATOFF; NEXTRAW + c->e[1] = ATOFF; NEXTRAW + c->e[4] = ATOFF; NEXTRAW + c->e[7] = ATOFF; NEXTRAW + c->e[2] = ATOFF; NEXTRAW + c->e[5] = ATOFF; NEXTRAW + c->e[8] = ATOFF; NEXTRAW + b->c[0] = ATOFF; NEXTRAW + b->c[1] = ATOFF; NEXTRAW + b->c[2] = ATOFF; +} + +void VnUartPacket_parseReferenceFrameRotation(VnUartPacket *packet, mat3f* c) +{ + VnUartPacket_parseReferenceFrameRotationRaw((char*)packet->data, c); +} + +void VnUartPacket_parseReferenceFrameRotationRaw(char *packet, mat3f* c) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + c->e[0] = ATOFF; NEXTRAW + c->e[3] = ATOFF; NEXTRAW + c->e[6] = ATOFF; NEXTRAW + c->e[1] = ATOFF; NEXTRAW + c->e[4] = ATOFF; NEXTRAW + c->e[7] = ATOFF; NEXTRAW + c->e[2] = ATOFF; NEXTRAW + c->e[5] = ATOFF; NEXTRAW + c->e[8] = ATOFF; +} + +void VnUartPacket_parseYawPitchRollMagneticAccelerationAndAngularRates(VnUartPacket *packet, vec3f* yawPitchRoll, vec3f* mag, vec3f* accel, vec3f* gyro) +{ + VnUartPacket_parseYawPitchRollMagneticAccelerationAndAngularRatesRaw((char*)packet->data, yawPitchRoll, mag, accel, gyro); +} + +void VnUartPacket_parseYawPitchRollMagneticAccelerationAndAngularRatesRaw(char *packet, vec3f* yawPitchRoll, vec3f* mag, vec3f* accel, vec3f* gyro) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + yawPitchRoll->c[0] = ATOFF; NEXTRAW + yawPitchRoll->c[1] = ATOFF; NEXTRAW + yawPitchRoll->c[2] = ATOFF; NEXTRAW + mag->c[0] = ATOFF; NEXTRAW + mag->c[1] = ATOFF; NEXTRAW + mag->c[2] = ATOFF; NEXTRAW + accel->c[0] = ATOFF; NEXTRAW + accel->c[1] = ATOFF; NEXTRAW + accel->c[2] = ATOFF; NEXTRAW + gyro->c[0] = ATOFF; NEXTRAW + gyro->c[1] = ATOFF; NEXTRAW + gyro->c[2] = ATOFF; +} + +void VnUartPacket_parseCommunicationProtocolControl(VnUartPacket *packet, uint8_t* serialCount, uint8_t* serialStatus, uint8_t* spiCount, uint8_t* spiStatus, uint8_t* serialChecksum, uint8_t* spiChecksum, uint8_t* errorMode) +{ + VnUartPacket_parseCommunicationProtocolControlRaw((char*)packet->data, serialCount, serialStatus, spiCount, spiStatus, serialChecksum, spiChecksum, errorMode); +} + +void VnUartPacket_parseCommunicationProtocolControlRaw(char *packet, uint8_t* serialCount, uint8_t* serialStatus, uint8_t* spiCount, uint8_t* spiStatus, uint8_t* serialChecksum, uint8_t* spiChecksum, uint8_t* errorMode) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *serialCount = ATOU8; NEXTRAW + *serialStatus = ATOU8; NEXTRAW + *spiCount = ATOU8; NEXTRAW + *spiStatus = ATOU8; NEXTRAW + *serialChecksum = ATOU8; NEXTRAW + *spiChecksum = ATOU8; NEXTRAW + *errorMode = ATOU8; +} + +void VnUartPacket_parseSynchronizationControl(VnUartPacket *packet, uint8_t* syncInMode, uint8_t* syncInEdge, uint16_t* syncInSkipFactor, uint32_t* reserved1, uint8_t* syncOutMode, uint8_t* syncOutPolarity, uint16_t* syncOutSkipFactor, uint32_t* syncOutPulseWidth, uint32_t* reserved2) +{ + VnUartPacket_parseSynchronizationControlRaw((char*)packet->data, syncInMode, syncInEdge, syncInSkipFactor, reserved1, syncOutMode, syncOutPolarity, syncOutSkipFactor, syncOutPulseWidth, reserved2); +} + +void VnUartPacket_parseSynchronizationControlRaw(char *packet, uint8_t* syncInMode, uint8_t* syncInEdge, uint16_t* syncInSkipFactor, uint32_t* reserved1, uint8_t* syncOutMode, uint8_t* syncOutPolarity, uint16_t* syncOutSkipFactor, uint32_t* syncOutPulseWidth, uint32_t* reserved2) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *syncInMode = ATOU8; NEXTRAW + *syncInEdge = ATOU8; NEXTRAW + *syncInSkipFactor = ATOU16; NEXTRAW + *reserved1 = ATOU32; NEXTRAW + *syncOutMode = ATOU8; NEXTRAW + *syncOutPolarity = ATOU8; NEXTRAW + *syncOutSkipFactor = ATOU16; NEXTRAW + *syncOutPulseWidth = ATOU32; NEXTRAW + *reserved2 = ATOU32; +} + +void VnUartPacket_parseSynchronizationStatus(VnUartPacket *packet, uint32_t* syncInCount, uint32_t* syncInTime, uint32_t* syncOutCount) +{ + VnUartPacket_parseSynchronizationStatusRaw((char*)packet->data, syncInCount, syncInTime, syncOutCount); +} + +void VnUartPacket_parseSynchronizationStatusRaw(char *packet, uint32_t* syncInCount, uint32_t* syncInTime, uint32_t* syncOutCount) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *syncInCount = ATOU32; NEXTRAW + *syncInTime = ATOU32; NEXTRAW + *syncOutCount = ATOU32; +} + +void VnUartPacket_parseFilterBasicControl(VnUartPacket *packet, uint8_t* magMode, uint8_t* extMagMode, uint8_t* extAccMode, uint8_t* extGyroMode, vec3f* gyroLimit) +{ + VnUartPacket_parseFilterBasicControlRaw((char*)packet->data, magMode, extMagMode, extAccMode, extGyroMode, gyroLimit); +} + +void VnUartPacket_parseFilterBasicControlRaw(char *packet, uint8_t* magMode, uint8_t* extMagMode, uint8_t* extAccMode, uint8_t* extGyroMode, vec3f* gyroLimit) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *magMode = ATOU8; NEXTRAW + *extMagMode = ATOU8; NEXTRAW + *extAccMode = ATOU8; NEXTRAW + *extGyroMode = ATOU8; NEXTRAW + gyroLimit->c[0] = ATOFF; NEXTRAW + gyroLimit->c[1] = ATOFF; NEXTRAW + gyroLimit->c[2] = ATOFF; +} + +void VnUartPacket_parseVpeBasicControl(VnUartPacket *packet, uint8_t* enable, uint8_t* headingMode, uint8_t* filteringMode, uint8_t* tuningMode) +{ + VnUartPacket_parseVpeBasicControlRaw((char*)packet->data, enable, headingMode, filteringMode, tuningMode); +} + +void VnUartPacket_parseVpeBasicControlRaw(char *packet, uint8_t* enable, uint8_t* headingMode, uint8_t* filteringMode, uint8_t* tuningMode) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *enable = ATOU8; NEXTRAW + *headingMode = ATOU8; NEXTRAW + *filteringMode = ATOU8; NEXTRAW + *tuningMode = ATOU8; +} + +void VnUartPacket_parseVpeMagnetometerBasicTuning(VnUartPacket *packet, vec3f* baseTuning, vec3f* adaptiveTuning, vec3f* adaptiveFiltering) +{ + VnUartPacket_parseVpeMagnetometerBasicTuningRaw((char*)packet->data, baseTuning, adaptiveTuning, adaptiveFiltering); +} + +void VnUartPacket_parseVpeMagnetometerBasicTuningRaw(char *packet, vec3f* baseTuning, vec3f* adaptiveTuning, vec3f* adaptiveFiltering) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + baseTuning->c[0] = ATOFF; NEXTRAW + baseTuning->c[1] = ATOFF; NEXTRAW + baseTuning->c[2] = ATOFF; NEXTRAW + adaptiveTuning->c[0] = ATOFF; NEXTRAW + adaptiveTuning->c[1] = ATOFF; NEXTRAW + adaptiveTuning->c[2] = ATOFF; NEXTRAW + adaptiveFiltering->c[0] = ATOFF; NEXTRAW + adaptiveFiltering->c[1] = ATOFF; NEXTRAW + adaptiveFiltering->c[2] = ATOFF; +} + +void VnUartPacket_parseVpeMagnetometerAdvancedTuning(VnUartPacket *packet, vec3f* minFiltering, vec3f* maxFiltering, float* maxAdaptRate, float* disturbanceWindow, float* maxTuning) +{ + VnUartPacket_parseVpeMagnetometerAdvancedTuningRaw((char*)packet->data, minFiltering, maxFiltering, maxAdaptRate, disturbanceWindow, maxTuning); +} + +void VnUartPacket_parseVpeMagnetometerAdvancedTuningRaw(char *packet, vec3f* minFiltering, vec3f* maxFiltering, float* maxAdaptRate, float* disturbanceWindow, float* maxTuning) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + minFiltering->c[0] = ATOFF; NEXTRAW + minFiltering->c[1] = ATOFF; NEXTRAW + minFiltering->c[2] = ATOFF; NEXTRAW + maxFiltering->c[0] = ATOFF; NEXTRAW + maxFiltering->c[1] = ATOFF; NEXTRAW + maxFiltering->c[2] = ATOFF; NEXTRAW + *maxAdaptRate = ATOFF; NEXTRAW + *disturbanceWindow = ATOFF; NEXTRAW + *maxTuning = ATOFF; +} + +void VnUartPacket_parseVpeAccelerometerBasicTuning(VnUartPacket *packet, vec3f* baseTuning, vec3f* adaptiveTuning, vec3f* adaptiveFiltering) +{ + VnUartPacket_parseVpeAccelerometerBasicTuningRaw((char*)packet->data, baseTuning, adaptiveTuning, adaptiveFiltering); +} + +void VnUartPacket_parseVpeAccelerometerBasicTuningRaw(char *packet, vec3f* baseTuning, vec3f* adaptiveTuning, vec3f* adaptiveFiltering) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + baseTuning->c[0] = ATOFF; NEXTRAW + baseTuning->c[1] = ATOFF; NEXTRAW + baseTuning->c[2] = ATOFF; NEXTRAW + adaptiveTuning->c[0] = ATOFF; NEXTRAW + adaptiveTuning->c[1] = ATOFF; NEXTRAW + adaptiveTuning->c[2] = ATOFF; NEXTRAW + adaptiveFiltering->c[0] = ATOFF; NEXTRAW + adaptiveFiltering->c[1] = ATOFF; NEXTRAW + adaptiveFiltering->c[2] = ATOFF; +} + +void VnUartPacket_parseVpeAccelerometerAdvancedTuning(VnUartPacket *packet, vec3f* minFiltering, vec3f* maxFiltering, float* maxAdaptRate, float* disturbanceWindow, float* maxTuning) +{ + VnUartPacket_parseVpeAccelerometerAdvancedTuningRaw((char*)packet->data, minFiltering, maxFiltering, maxAdaptRate, disturbanceWindow, maxTuning); +} + +void VnUartPacket_parseVpeAccelerometerAdvancedTuningRaw(char *packet, vec3f* minFiltering, vec3f* maxFiltering, float* maxAdaptRate, float* disturbanceWindow, float* maxTuning) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + minFiltering->c[0] = ATOFF; NEXTRAW + minFiltering->c[1] = ATOFF; NEXTRAW + minFiltering->c[2] = ATOFF; NEXTRAW + maxFiltering->c[0] = ATOFF; NEXTRAW + maxFiltering->c[1] = ATOFF; NEXTRAW + maxFiltering->c[2] = ATOFF; NEXTRAW + *maxAdaptRate = ATOFF; NEXTRAW + *disturbanceWindow = ATOFF; NEXTRAW + *maxTuning = ATOFF; +} + +void VnUartPacket_parseVpeGyroBasicTuning(VnUartPacket *packet, vec3f* angularWalkVariance, vec3f* baseTuning, vec3f* adaptiveTuning) +{ + VnUartPacket_parseVpeGyroBasicTuningRaw((char*)packet->data, angularWalkVariance, baseTuning, adaptiveTuning); +} + +void VnUartPacket_parseVpeGyroBasicTuningRaw(char *packet, vec3f* angularWalkVariance, vec3f* baseTuning, vec3f* adaptiveTuning) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + angularWalkVariance->c[0] = ATOFF; NEXTRAW + angularWalkVariance->c[1] = ATOFF; NEXTRAW + angularWalkVariance->c[2] = ATOFF; NEXTRAW + baseTuning->c[0] = ATOFF; NEXTRAW + baseTuning->c[1] = ATOFF; NEXTRAW + baseTuning->c[2] = ATOFF; NEXTRAW + adaptiveTuning->c[0] = ATOFF; NEXTRAW + adaptiveTuning->c[1] = ATOFF; NEXTRAW + adaptiveTuning->c[2] = ATOFF; +} + +void VnUartPacket_parseFilterStartupGyroBias(VnUartPacket *packet, vec3f* bias) +{ + VnUartPacket_parseFilterStartupGyroBiasRaw((char*)packet->data, bias); +} + +void VnUartPacket_parseFilterStartupGyroBiasRaw(char *packet, vec3f* bias) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + bias->c[0] = ATOFF; NEXTRAW + bias->c[1] = ATOFF; NEXTRAW + bias->c[2] = ATOFF; +} + +void VnUartPacket_parseMagnetometerCalibrationControl(VnUartPacket *packet, uint8_t* hsiMode, uint8_t* hsiOutput, uint8_t* convergeRate) +{ + VnUartPacket_parseMagnetometerCalibrationControlRaw((char*)packet->data, hsiMode, hsiOutput, convergeRate); +} + +void VnUartPacket_parseMagnetometerCalibrationControlRaw(char *packet, uint8_t* hsiMode, uint8_t* hsiOutput, uint8_t* convergeRate) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *hsiMode = ATOU8; NEXTRAW + *hsiOutput = ATOU8; NEXTRAW + *convergeRate = ATOU8; +} + +void VnUartPacket_parseCalculatedMagnetometerCalibration(VnUartPacket *packet, mat3f* c, vec3f* b) +{ + VnUartPacket_parseCalculatedMagnetometerCalibrationRaw((char*)packet->data, c, b); +} + +void VnUartPacket_parseCalculatedMagnetometerCalibrationRaw(char *packet, mat3f* c, vec3f* b) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + c->e[0] = ATOFF; NEXTRAW + c->e[3] = ATOFF; NEXTRAW + c->e[6] = ATOFF; NEXTRAW + c->e[1] = ATOFF; NEXTRAW + c->e[4] = ATOFF; NEXTRAW + c->e[7] = ATOFF; NEXTRAW + c->e[2] = ATOFF; NEXTRAW + c->e[5] = ATOFF; NEXTRAW + c->e[8] = ATOFF; NEXTRAW + b->c[0] = ATOFF; NEXTRAW + b->c[1] = ATOFF; NEXTRAW + b->c[2] = ATOFF; +} + +void VnUartPacket_parseIndoorHeadingModeControl(VnUartPacket *packet, float* maxRateError, uint8_t* reserved1) +{ + VnUartPacket_parseIndoorHeadingModeControlRaw((char*)packet->data, maxRateError, reserved1); +} + +void VnUartPacket_parseIndoorHeadingModeControlRaw(char *packet, float* maxRateError, uint8_t* reserved1) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *maxRateError = ATOFF; NEXTRAW + *reserved1 = ATOU8; +} + +void VnUartPacket_parseVelocityCompensationMeasurement(VnUartPacket *packet, vec3f* velocity) +{ + VnUartPacket_parseVelocityCompensationMeasurementRaw((char*)packet->data, velocity); +} + +void VnUartPacket_parseVelocityCompensationMeasurementRaw(char *packet, vec3f* velocity) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + velocity->c[0] = ATOFF; NEXTRAW + velocity->c[1] = ATOFF; NEXTRAW + velocity->c[2] = ATOFF; +} + +void VnUartPacket_parseVelocityCompensationControl(VnUartPacket *packet, uint8_t* mode, float* velocityTuning, float* rateTuning) +{ + VnUartPacket_parseVelocityCompensationControlRaw((char*)packet->data, mode, velocityTuning, rateTuning); +} + +void VnUartPacket_parseVelocityCompensationControlRaw(char *packet, uint8_t* mode, float* velocityTuning, float* rateTuning) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *mode = ATOU8; NEXTRAW + *velocityTuning = ATOFF; NEXTRAW + *rateTuning = ATOFF; +} + +void VnUartPacket_parseVelocityCompensationStatus(VnUartPacket *packet, float* x, float* xDot, vec3f* accelOffset, vec3f* omega) +{ + VnUartPacket_parseVelocityCompensationStatusRaw((char*)packet->data, x, xDot, accelOffset, omega); +} + +void VnUartPacket_parseVelocityCompensationStatusRaw(char *packet, float* x, float* xDot, vec3f* accelOffset, vec3f* omega) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *x = ATOFF; NEXTRAW + *xDot = ATOFF; NEXTRAW + accelOffset->c[0] = ATOFF; NEXTRAW + accelOffset->c[1] = ATOFF; NEXTRAW + accelOffset->c[2] = ATOFF; NEXTRAW + omega->c[0] = ATOFF; NEXTRAW + omega->c[1] = ATOFF; NEXTRAW + omega->c[2] = ATOFF; +} + +void VnUartPacket_parseImuMeasurements(VnUartPacket *packet, vec3f* mag, vec3f* accel, vec3f* gyro, float* temp, float* pressure) +{ + VnUartPacket_parseImuMeasurementsRaw((char*)packet->data, mag, accel, gyro, temp, pressure); +} + +void VnUartPacket_parseImuMeasurementsRaw(char *packet, vec3f* mag, vec3f* accel, vec3f* gyro, float* temp, float* pressure) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + mag->c[0] = ATOFF; NEXTRAW + mag->c[1] = ATOFF; NEXTRAW + mag->c[2] = ATOFF; NEXTRAW + accel->c[0] = ATOFF; NEXTRAW + accel->c[1] = ATOFF; NEXTRAW + accel->c[2] = ATOFF; NEXTRAW + gyro->c[0] = ATOFF; NEXTRAW + gyro->c[1] = ATOFF; NEXTRAW + gyro->c[2] = ATOFF; NEXTRAW + *temp = ATOFF; NEXTRAW + *pressure = ATOFF; +} + +void VnUartPacket_parseGpsConfiguration(VnUartPacket *packet, uint8_t* mode, uint8_t* ppsSource, uint8_t* reserved1, uint8_t* reserved2, uint8_t* reserved3) +{ + VnUartPacket_parseGpsConfigurationRaw((char*)packet->data, mode, ppsSource, reserved1, reserved2, reserved3); +} + +void VnUartPacket_parseGpsConfigurationRaw(char *packet, uint8_t* mode, uint8_t* ppsSource, uint8_t* reserved1, uint8_t* reserved2, uint8_t* reserved3) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *mode = ATOU8; NEXTRAW + *ppsSource = ATOU8; NEXTRAW + *reserved1 = ATOU8; NEXTRAW + *reserved2 = ATOU8; NEXTRAW + *reserved3 = ATOU8; +} + +void VnUartPacket_parseGpsAntennaOffset(VnUartPacket *packet, vec3f* position) +{ + VnUartPacket_parseGpsAntennaOffsetRaw((char*)packet->data, position); +} + +void VnUartPacket_parseGpsAntennaOffsetRaw(char *packet, vec3f* position) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + position->c[0] = ATOFF; NEXTRAW + position->c[1] = ATOFF; NEXTRAW + position->c[2] = ATOFF; +} + +void VnUartPacket_parseGpsSolutionLla(VnUartPacket *packet, double* time, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, vec3d* lla, vec3f* nedVel, vec3f* nedAcc, float* speedAcc, float* timeAcc) +{ + VnUartPacket_parseGpsSolutionLlaRaw((char*)packet->data, time, week, gpsFix, numSats, lla, nedVel, nedAcc, speedAcc, timeAcc); +} + +void VnUartPacket_parseGpsSolutionLlaRaw(char *packet, double* time, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, vec3d* lla, vec3f* nedVel, vec3f* nedAcc, float* speedAcc, float* timeAcc) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *time = ATOFD; NEXTRAW + *week = ATOU16; NEXTRAW + *gpsFix = ATOU8; NEXTRAW + *numSats = ATOU8; NEXTRAW + lla->c[0] = ATOFD; NEXTRAW + lla->c[1] = ATOFD; NEXTRAW + lla->c[2] = ATOFD; NEXTRAW + nedVel->c[0] = ATOFF; NEXTRAW + nedVel->c[1] = ATOFF; NEXTRAW + nedVel->c[2] = ATOFF; NEXTRAW + nedAcc->c[0] = ATOFF; NEXTRAW + nedAcc->c[1] = ATOFF; NEXTRAW + nedAcc->c[2] = ATOFF; NEXTRAW + *speedAcc = ATOFF; NEXTRAW + *timeAcc = ATOFF; +} + +void VnUartPacket_parseGpsSolutionEcef(VnUartPacket *packet, double* tow, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, vec3d* position, vec3f* velocity, vec3f* posAcc, float* speedAcc, float* timeAcc) +{ + VnUartPacket_parseGpsSolutionEcefRaw((char*)packet->data, tow, week, gpsFix, numSats, position, velocity, posAcc, speedAcc, timeAcc); +} + +void VnUartPacket_parseGpsSolutionEcefRaw(char *packet, double* tow, uint16_t* week, uint8_t* gpsFix, uint8_t* numSats, vec3d* position, vec3f* velocity, vec3f* posAcc, float* speedAcc, float* timeAcc) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *tow = ATOFD; NEXTRAW + *week = ATOU16; NEXTRAW + *gpsFix = ATOU8; NEXTRAW + *numSats = ATOU8; NEXTRAW + position->c[0] = ATOFD; NEXTRAW + position->c[1] = ATOFD; NEXTRAW + position->c[2] = ATOFD; NEXTRAW + velocity->c[0] = ATOFF; NEXTRAW + velocity->c[1] = ATOFF; NEXTRAW + velocity->c[2] = ATOFF; NEXTRAW + posAcc->c[0] = ATOFF; NEXTRAW + posAcc->c[1] = ATOFF; NEXTRAW + posAcc->c[2] = ATOFF; NEXTRAW + *speedAcc = ATOFF; NEXTRAW + *timeAcc = ATOFF; +} + +void VnUartPacket_parseInsSolutionLla(VnUartPacket *packet, double* time, uint16_t* week, uint16_t* status, vec3f* yawPitchRoll, vec3d* position, vec3f* nedVel, float* attUncertainty, float* posUncertainty, float* velUncertainty) +{ + VnUartPacket_parseInsSolutionLlaRaw((char*)packet->data, time, week, status, yawPitchRoll, position, nedVel, attUncertainty, posUncertainty, velUncertainty); +} + +void VnUartPacket_parseInsSolutionLlaRaw(char *packet, double* time, uint16_t* week, uint16_t* status, vec3f* yawPitchRoll, vec3d* position, vec3f* nedVel, float* attUncertainty, float* posUncertainty, float* velUncertainty) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *time = ATOFD; NEXTRAW + *week = ATOU16; NEXTRAW + *status = ATOU16X; NEXTRAW + yawPitchRoll->c[0] = ATOFF; NEXTRAW + yawPitchRoll->c[1] = ATOFF; NEXTRAW + yawPitchRoll->c[2] = ATOFF; NEXTRAW + position->c[0] = ATOFD; NEXTRAW + position->c[1] = ATOFD; NEXTRAW + position->c[2] = ATOFD; NEXTRAW + nedVel->c[0] = ATOFF; NEXTRAW + nedVel->c[1] = ATOFF; NEXTRAW + nedVel->c[2] = ATOFF; NEXTRAW + *attUncertainty = ATOFF; NEXTRAW + *posUncertainty = ATOFF; NEXTRAW + *velUncertainty = ATOFF; +} + +void VnUartPacket_parseInsSolutionEcef(VnUartPacket *packet, double* time, uint16_t* week, uint16_t* status, vec3f* yawPitchRoll, vec3d* position, vec3f* velocity, float* attUncertainty, float* posUncertainty, float* velUncertainty) +{ + VnUartPacket_parseInsSolutionEcefRaw((char*)packet->data, time, week, status, yawPitchRoll, position, velocity, attUncertainty, posUncertainty, velUncertainty); +} + +void VnUartPacket_parseInsSolutionEcefRaw(char *packet, double* time, uint16_t* week, uint16_t* status, vec3f* yawPitchRoll, vec3d* position, vec3f* velocity, float* attUncertainty, float* posUncertainty, float* velUncertainty) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *time = ATOFD; NEXTRAW + *week = ATOU16; NEXTRAW + *status = ATOU16X; NEXTRAW + yawPitchRoll->c[0] = ATOFF; NEXTRAW + yawPitchRoll->c[1] = ATOFF; NEXTRAW + yawPitchRoll->c[2] = ATOFF; NEXTRAW + position->c[0] = ATOFD; NEXTRAW + position->c[1] = ATOFD; NEXTRAW + position->c[2] = ATOFD; NEXTRAW + velocity->c[0] = ATOFF; NEXTRAW + velocity->c[1] = ATOFF; NEXTRAW + velocity->c[2] = ATOFF; NEXTRAW + *attUncertainty = ATOFF; NEXTRAW + *posUncertainty = ATOFF; NEXTRAW + *velUncertainty = ATOFF; +} + +void VnUartPacket_parseInsBasicConfiguration(VnUartPacket *packet, uint8_t* scenario, uint8_t* ahrsAiding, uint8_t* estBaseline, uint8_t* resv2) +{ + VnUartPacket_parseInsBasicConfigurationRaw((char*)packet->data, scenario, ahrsAiding, estBaseline, resv2); +} + +void VnUartPacket_parseInsBasicConfigurationRaw(char *packet, uint8_t* scenario, uint8_t* ahrsAiding, uint8_t* estBaseline, uint8_t* resv2) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *scenario = ATOU8; NEXTRAW + *ahrsAiding = ATOU8; NEXTRAW + *estBaseline = ATOU8; NEXTRAW + *resv2 = ATOU8; +} + +void VnUartPacket_parseInsAdvancedConfiguration(VnUartPacket *packet, uint8_t* useMag, uint8_t* usePres, uint8_t* posAtt, uint8_t* velAtt, uint8_t* velBias, uint8_t* useFoam, uint8_t* gpsCovType, uint8_t* velCount, float* velInit, float* moveOrigin, float* gpsTimeout, float* deltaLimitPos, float* deltaLimitVel, float* minPosUncertainty, float* minVelUncertainty) +{ + VnUartPacket_parseInsAdvancedConfigurationRaw((char*)packet->data, useMag, usePres, posAtt, velAtt, velBias, useFoam, gpsCovType, velCount, velInit, moveOrigin, gpsTimeout, deltaLimitPos, deltaLimitVel, minPosUncertainty, minVelUncertainty); +} + +void VnUartPacket_parseInsAdvancedConfigurationRaw(char *packet, uint8_t* useMag, uint8_t* usePres, uint8_t* posAtt, uint8_t* velAtt, uint8_t* velBias, uint8_t* useFoam, uint8_t* gpsCovType, uint8_t* velCount, float* velInit, float* moveOrigin, float* gpsTimeout, float* deltaLimitPos, float* deltaLimitVel, float* minPosUncertainty, float* minVelUncertainty) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *useMag = ATOU8; NEXTRAW + *usePres = ATOU8; NEXTRAW + *posAtt = ATOU8; NEXTRAW + *velAtt = ATOU8; NEXTRAW + *velBias = ATOU8; NEXTRAW + *useFoam = ATOU8; NEXTRAW + *gpsCovType = ATOU8; NEXTRAW + *velCount = ATOU8; NEXTRAW + *velInit = ATOFF; NEXTRAW + *moveOrigin = ATOFF; NEXTRAW + *gpsTimeout = ATOFF; NEXTRAW + *deltaLimitPos = ATOFF; NEXTRAW + *deltaLimitVel = ATOFF; NEXTRAW + *minPosUncertainty = ATOFF; NEXTRAW + *minVelUncertainty = ATOFF; +} + +void VnUartPacket_parseInsStateLla(VnUartPacket *packet, vec3f* yawPitchRoll, vec3d* position, vec3f* velocity, vec3f* accel, vec3f* angularRate) +{ + VnUartPacket_parseInsStateLlaRaw((char*)packet->data, yawPitchRoll, position, velocity, accel, angularRate); +} + +void VnUartPacket_parseInsStateLlaRaw(char *packet, vec3f* yawPitchRoll, vec3d* position, vec3f* velocity, vec3f* accel, vec3f* angularRate) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + yawPitchRoll->c[0] = ATOFF; NEXTRAW + yawPitchRoll->c[1] = ATOFF; NEXTRAW + yawPitchRoll->c[2] = ATOFF; NEXTRAW + position->c[0] = ATOFD; NEXTRAW + position->c[1] = ATOFD; NEXTRAW + position->c[2] = ATOFD; NEXTRAW + velocity->c[0] = ATOFF; NEXTRAW + velocity->c[1] = ATOFF; NEXTRAW + velocity->c[2] = ATOFF; NEXTRAW + accel->c[0] = ATOFF; NEXTRAW + accel->c[1] = ATOFF; NEXTRAW + accel->c[2] = ATOFF; NEXTRAW + angularRate->c[0] = ATOFF; NEXTRAW + angularRate->c[1] = ATOFF; NEXTRAW + angularRate->c[2] = ATOFF; +} + +void VnUartPacket_parseInsStateEcef(VnUartPacket *packet, vec3f* yawPitchRoll, vec3d* position, vec3f* velocity, vec3f* accel, vec3f* angularRate) +{ + VnUartPacket_parseInsStateEcefRaw((char*)packet->data, yawPitchRoll, position, velocity, accel, angularRate); +} + +void VnUartPacket_parseInsStateEcefRaw(char *packet, vec3f* yawPitchRoll, vec3d* position, vec3f* velocity, vec3f* accel, vec3f* angularRate) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + yawPitchRoll->c[0] = ATOFF; NEXTRAW + yawPitchRoll->c[1] = ATOFF; NEXTRAW + yawPitchRoll->c[2] = ATOFF; NEXTRAW + position->c[0] = ATOFD; NEXTRAW + position->c[1] = ATOFD; NEXTRAW + position->c[2] = ATOFD; NEXTRAW + velocity->c[0] = ATOFF; NEXTRAW + velocity->c[1] = ATOFF; NEXTRAW + velocity->c[2] = ATOFF; NEXTRAW + accel->c[0] = ATOFF; NEXTRAW + accel->c[1] = ATOFF; NEXTRAW + accel->c[2] = ATOFF; NEXTRAW + angularRate->c[0] = ATOFF; NEXTRAW + angularRate->c[1] = ATOFF; NEXTRAW + angularRate->c[2] = ATOFF; +} + +void VnUartPacket_parseStartupFilterBiasEstimate(VnUartPacket *packet, vec3f* gyroBias, vec3f* accelBias, float* pressureBias) +{ + VnUartPacket_parseStartupFilterBiasEstimateRaw((char*)packet->data, gyroBias, accelBias, pressureBias); +} + +void VnUartPacket_parseStartupFilterBiasEstimateRaw(char *packet, vec3f* gyroBias, vec3f* accelBias, float* pressureBias) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + gyroBias->c[0] = ATOFF; NEXTRAW + gyroBias->c[1] = ATOFF; NEXTRAW + gyroBias->c[2] = ATOFF; NEXTRAW + accelBias->c[0] = ATOFF; NEXTRAW + accelBias->c[1] = ATOFF; NEXTRAW + accelBias->c[2] = ATOFF; NEXTRAW + *pressureBias = ATOFF; +} + +void VnUartPacket_parseDeltaThetaAndDeltaVelocity(VnUartPacket *packet, float* deltaTime, vec3f* deltaTheta, vec3f* deltaVelocity) +{ + VnUartPacket_parseDeltaThetaAndDeltaVelocityRaw((char*)packet->data, deltaTime, deltaTheta, deltaVelocity); +} + +void VnUartPacket_parseDeltaThetaAndDeltaVelocityRaw(char *packet, float* deltaTime, vec3f* deltaTheta, vec3f* deltaVelocity) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *deltaTime = ATOFF; NEXTRAW + deltaTheta->c[0] = ATOFF; NEXTRAW + deltaTheta->c[1] = ATOFF; NEXTRAW + deltaTheta->c[2] = ATOFF; NEXTRAW + deltaVelocity->c[0] = ATOFF; NEXTRAW + deltaVelocity->c[1] = ATOFF; NEXTRAW + deltaVelocity->c[2] = ATOFF; +} + +void VnUartPacket_parseDeltaThetaAndDeltaVelocityConfiguration(VnUartPacket *packet, uint8_t* integrationFrame, uint8_t* gyroCompensation, uint8_t* accelCompensation, uint8_t* reserved1, uint16_t* reserved2) +{ + VnUartPacket_parseDeltaThetaAndDeltaVelocityConfigurationRaw((char*)packet->data, integrationFrame, gyroCompensation, accelCompensation, reserved1, reserved2); +} + +void VnUartPacket_parseDeltaThetaAndDeltaVelocityConfigurationRaw(char *packet, uint8_t* integrationFrame, uint8_t* gyroCompensation, uint8_t* accelCompensation, uint8_t* reserved1, uint16_t* reserved2) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *integrationFrame = ATOU8; NEXTRAW + *gyroCompensation = ATOU8; NEXTRAW + *accelCompensation = ATOU8; NEXTRAW + *reserved1 = ATOU8; NEXTRAW + *reserved2 = ATOU16; +} + +void VnUartPacket_parseReferenceVectorConfiguration(VnUartPacket *packet, uint8_t* useMagModel, uint8_t* useGravityModel, uint8_t* resv1, uint8_t* resv2, uint32_t* recalcThreshold, float* year, vec3d* position) +{ + VnUartPacket_parseReferenceVectorConfigurationRaw((char*)packet->data, useMagModel, useGravityModel, resv1, resv2, recalcThreshold, year, position); +} + +void VnUartPacket_parseReferenceVectorConfigurationRaw(char *packet, uint8_t* useMagModel, uint8_t* useGravityModel, uint8_t* resv1, uint8_t* resv2, uint32_t* recalcThreshold, float* year, vec3d* position) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *useMagModel = ATOU8; NEXTRAW + *useGravityModel = ATOU8; NEXTRAW + *resv1 = ATOU8; NEXTRAW + *resv2 = ATOU8; NEXTRAW + *recalcThreshold = ATOU32; NEXTRAW + *year = ATOFF; NEXTRAW + position->c[0] = ATOFD; NEXTRAW + position->c[1] = ATOFD; NEXTRAW + position->c[2] = ATOFD; +} + +void VnUartPacket_parseGyroCompensation(VnUartPacket *packet, mat3f* c, vec3f* b) +{ + VnUartPacket_parseGyroCompensationRaw((char*)packet->data, c, b); +} + +void VnUartPacket_parseGyroCompensationRaw(char *packet, mat3f* c, vec3f* b) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + c->e[0] = ATOFF; NEXTRAW + c->e[3] = ATOFF; NEXTRAW + c->e[6] = ATOFF; NEXTRAW + c->e[1] = ATOFF; NEXTRAW + c->e[4] = ATOFF; NEXTRAW + c->e[7] = ATOFF; NEXTRAW + c->e[2] = ATOFF; NEXTRAW + c->e[5] = ATOFF; NEXTRAW + c->e[8] = ATOFF; NEXTRAW + b->c[0] = ATOFF; NEXTRAW + b->c[1] = ATOFF; NEXTRAW + b->c[2] = ATOFF; +} + +void VnUartPacket_parseImuFilteringConfiguration(VnUartPacket *packet, uint16_t* magWindowSize, uint16_t* accelWindowSize, uint16_t* gyroWindowSize, uint16_t* tempWindowSize, uint16_t* presWindowSize, uint8_t* magFilterMode, uint8_t* accelFilterMode, uint8_t* gyroFilterMode, uint8_t* tempFilterMode, uint8_t* presFilterMode) +{ + VnUartPacket_parseImuFilteringConfigurationRaw((char*)packet->data, magWindowSize, accelWindowSize, gyroWindowSize, tempWindowSize, presWindowSize, magFilterMode, accelFilterMode, gyroFilterMode, tempFilterMode, presFilterMode); +} + +void VnUartPacket_parseImuFilteringConfigurationRaw(char *packet, uint16_t* magWindowSize, uint16_t* accelWindowSize, uint16_t* gyroWindowSize, uint16_t* tempWindowSize, uint16_t* presWindowSize, uint8_t* magFilterMode, uint8_t* accelFilterMode, uint8_t* gyroFilterMode, uint8_t* tempFilterMode, uint8_t* presFilterMode) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *magWindowSize = ATOU16; NEXTRAW + *accelWindowSize = ATOU16; NEXTRAW + *gyroWindowSize = ATOU16; NEXTRAW + *tempWindowSize = ATOU16; NEXTRAW + *presWindowSize = ATOU16; NEXTRAW + *magFilterMode = ATOU8; NEXTRAW + *accelFilterMode = ATOU8; NEXTRAW + *gyroFilterMode = ATOU8; NEXTRAW + *tempFilterMode = ATOU8; NEXTRAW + *presFilterMode = ATOU8; +} + +void VnUartPacket_parseGpsCompassBaseline(VnUartPacket *packet, vec3f* position, vec3f* uncertainty) +{ + VnUartPacket_parseGpsCompassBaselineRaw((char*)packet->data, position, uncertainty); +} + +void VnUartPacket_parseGpsCompassBaselineRaw(char *packet, vec3f* position, vec3f* uncertainty) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + position->c[0] = ATOFF; NEXTRAW + position->c[1] = ATOFF; NEXTRAW + position->c[2] = ATOFF; NEXTRAW + uncertainty->c[0] = ATOFF; NEXTRAW + uncertainty->c[1] = ATOFF; NEXTRAW + uncertainty->c[2] = ATOFF; +} + +void VnUartPacket_parseGpsCompassEstimatedBaseline(VnUartPacket *packet, uint8_t* estBaselineUsed, uint8_t* resv, uint16_t* numMeas, vec3f* position, vec3f* uncertainty) +{ + VnUartPacket_parseGpsCompassEstimatedBaselineRaw((char*)packet->data, estBaselineUsed, resv, numMeas, position, uncertainty); +} + +void VnUartPacket_parseGpsCompassEstimatedBaselineRaw(char *packet, uint8_t* estBaselineUsed, uint8_t* resv, uint16_t* numMeas, vec3f* position, vec3f* uncertainty) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *estBaselineUsed = ATOU8; NEXTRAW + *resv = ATOU8; NEXTRAW + *numMeas = ATOU16; NEXTRAW + position->c[0] = ATOFF; NEXTRAW + position->c[1] = ATOFF; NEXTRAW + position->c[2] = ATOFF; NEXTRAW + uncertainty->c[0] = ATOFF; NEXTRAW + uncertainty->c[1] = ATOFF; NEXTRAW + uncertainty->c[2] = ATOFF; +} + +void VnUartPacket_parseImuRateConfiguration(VnUartPacket *packet, uint16_t* imuRate, uint16_t* navDivisor, float* filterTargetRate, float* filterMinRate) +{ + VnUartPacket_parseImuRateConfigurationRaw((char*)packet->data, imuRate, navDivisor, filterTargetRate, filterMinRate); +} + +void VnUartPacket_parseImuRateConfigurationRaw(char *packet, uint16_t* imuRate, uint16_t* navDivisor, float* filterTargetRate, float* filterMinRate) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + *imuRate = ATOU16; NEXTRAW + *navDivisor = ATOU16; NEXTRAW + *filterTargetRate = ATOFF; NEXTRAW + *filterMinRate = ATOFF; +} + +void VnUartPacket_parseYawPitchRollTrueBodyAccelerationAndAngularRates(VnUartPacket *packet, vec3f* yawPitchRoll, vec3f* bodyAccel, vec3f* gyro) +{ + VnUartPacket_parseYawPitchRollTrueBodyAccelerationAndAngularRatesRaw((char*)packet->data, yawPitchRoll, bodyAccel, gyro); +} + +void VnUartPacket_parseYawPitchRollTrueBodyAccelerationAndAngularRatesRaw(char *packet, vec3f* yawPitchRoll, vec3f* bodyAccel, vec3f* gyro) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + yawPitchRoll->c[0] = ATOFF; NEXTRAW + yawPitchRoll->c[1] = ATOFF; NEXTRAW + yawPitchRoll->c[2] = ATOFF; NEXTRAW + bodyAccel->c[0] = ATOFF; NEXTRAW + bodyAccel->c[1] = ATOFF; NEXTRAW + bodyAccel->c[2] = ATOFF; NEXTRAW + gyro->c[0] = ATOFF; NEXTRAW + gyro->c[1] = ATOFF; NEXTRAW + gyro->c[2] = ATOFF; +} + +void VnUartPacket_parseYawPitchRollTrueInertialAccelerationAndAngularRates(VnUartPacket *packet, vec3f* yawPitchRoll, vec3f* inertialAccel, vec3f* gyro) +{ + VnUartPacket_parseYawPitchRollTrueInertialAccelerationAndAngularRatesRaw((char*)packet->data, yawPitchRoll, inertialAccel, gyro); +} + +void VnUartPacket_parseYawPitchRollTrueInertialAccelerationAndAngularRatesRaw(char *packet, vec3f* yawPitchRoll, vec3f* inertialAccel, vec3f* gyro) +{ + size_t packetIndex; + char *result = (char*)VnUartPacket_startAsciiResponsePacketParse((uint8_t*)packet, &packetIndex); + + yawPitchRoll->c[0] = ATOFF; NEXTRAW + yawPitchRoll->c[1] = ATOFF; NEXTRAW + yawPitchRoll->c[2] = ATOFF; NEXTRAW + inertialAccel->c[0] = ATOFF; NEXTRAW + inertialAccel->c[1] = ATOFF; NEXTRAW + inertialAccel->c[2] = ATOFF; NEXTRAW + gyro->c[0] = ATOFF; NEXTRAW + gyro->c[1] = ATOFF; NEXTRAW + gyro->c[2] = ATOFF; +} + +void strFromVnAsciiAsync(char *out, VnAsciiAsync 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 VNOFF: + strcpy(out, "Off"); + break; + case VNYPR: + strcpy(out, "Yaw, Pitch, Roll"); + break; + case VNQTN: + strcpy(out, "Quaterion"); + break; + #ifdef EXTRA + case VNQTM: + strcpy(out, "Quaternion and Magnetic"); + break; + case VNQTA: + strcpy(out, "Quaternion and Acceleration"); + break; + case VNQTR: + strcpy(out, "Quaternion and Angular Rates"); + break; + case VNQMA: + strcpy(out, "Quaternion, Magnetic and Acceleration"); + break; + case VNQAR: + strcpy(out, "Quaternion, Acceleration and Angular Rates"); + break; + #endif + case VNQMR: + strcpy(out, "Quaternion, Magnetic, Acceleration and Angular Rates"); + break; + #ifdef EXTRA + case VNDCM: + strcpy(out, "Directional Cosine Orientation Matrix"); + break; + #endif + case VNMAG: + strcpy(out, "Magnetic Measurements"); + break; + case VNACC: + strcpy(out, "Acceleration Measurements"); + break; + case VNGYR: + strcpy(out, "Angular Rate Measurements"); + break; + case VNMAR: + strcpy(out, "Magnetic, Acceleration, and Angular Rate Measurements"); + break; + case VNYMR: + strcpy(out, "Yaw, Pitch, Roll, Magnetic, Acceleration, and Angular Rate Measurements"); + break; + #ifdef EXTRA + case VNYCM: + strcpy(out, "Yaw, Pitch, Roll, and Calibrated Measurements"); + break; + #endif + case VNYBA: + strcpy(out, "Yaw, Pitch, Roll, Body True Acceleration"); + break; + case VNYIA: + strcpy(out, "Yaw, Pitch, Roll, Inertial True Acceleration"); + break; + #ifdef EXTRA + case VNICM: + strcpy(out, "Yaw, Pitch, Roll, Inertial Magnetic/Acceleration, and Angular Rate Measurements"); + break; + #endif + case VNIMU: + strcpy(out, "Calibrated Inertial Measurements"); + break; + case VNGPS: + strcpy(out, "GPS LLA"); + break; + case VNGPE: + strcpy(out, "GPS ECEF"); + break; + case VNINS: + strcpy(out, "INS LLA solution"); + break; + case VNINE: + strcpy(out, "INS ECEF solution"); + break; + case VNISL: + strcpy(out, "INS LLA 2 solution"); + break; + case VNISE: + strcpy(out, "INS ECEF 2 solution"); + break; + case VNDTV: + strcpy(out, "Delta Theta and Delta Velocity"); + break; + case VNRTK: + strcpy(out, "RTK"); + break; +#ifdef EXTRA + case VNRAW: + strcpy(out, "Raw Voltage Measurements"); + break; + case VNCMV: + strcpy(out, "Calibrated Measurements"); + break; + case VNSTV: + strcpy(out, "Kalman Filter State Vector"); + break; + case VNCOV: + strcpy(out, "Kalman Filter Covariance Matrix Diagonal"); + break; + #endif + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/protocol/upackf.c b/src/drivers/ins/vectornav/libvnc/src/vn/protocol/upackf.c new file mode 100644 index 0000000000..98763ec4cb --- /dev/null +++ b/src/drivers/ins/vectornav/libvnc/src/vn/protocol/upackf.c @@ -0,0 +1,438 @@ +#include "vn/protocol/upackf.h" + +#include +/*#include +#include */ + +#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; +} diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/sensors.c b/src/drivers/ins/vectornav/libvnc/src/vn/sensors.c new file mode 100644 index 0000000000..6db5efc71e --- /dev/null +++ b/src/drivers/ins/vectornav/libvnc/src/vn/sensors.c @@ -0,0 +1,3526 @@ +#include "vn/sensors.h" +#include "vn/error.h" +#include "vn/util.h" + +#include +#include +#include +#include + +#include "vn/xplat/time.h" + +#define UNUSED(x) (void)(sizeof(x)) + +#define DEFAULT_RESPONSE_TIMEOUT_MS 500 +#define DEFAULT_RETRANSMIT_DELAY_MS 200 +#define DEFAULT_READ_BUFFER_SIZE 256 +#define COMMAND_MAX_LENGTH 0x100 + +void VnSensor_dataReceivedHandler(void *userData); +void VnSensor_packetFoundHandler(void *userData, VnUartPacket *packet, size_t runningIndex); + +VnError VnSensor_transactionNoFinalize( + VnSensor *s, + char *toSend, + size_t toSendLength, + bool waitForReply, + char *responseBuffer, + size_t *responseSize); + +VnError VnSensor_transactionNoFinalizeWithTiming( + VnSensor *s, + char *toSend, + size_t toSendLength, + bool waitForReply, + char *responseBuffer, + size_t* responseBufferSize, + uint16_t responseTimeoutMs, + uint16_t retransmitDelayMs); + + + +void strFromSensorError(char *out, SensorError 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 ERR_HARD_FAULT: + strcpy(out, "HardFault"); + break; + case ERR_SERIAL_BUFFER_OVERFLOW: + strcpy(out, "SerialBufferOverflow"); + break; + case ERR_INVALID_CHECKSUM: + strcpy(out, "InvalidChecksum"); + break; + case ERR_INVALID_COMMAND: + strcpy(out, "InvalidCommand"); + break; + case ERR_NOT_ENOUGH_PARAMETERS: + strcpy(out, "NotEnoughParameters"); + break; + case ERR_TOO_MANY_PARAMETERS: + strcpy(out, "TooManyParameters"); + break; + case ERR_INVALID_PARAMETER: + strcpy(out, "InvalidParameter"); + break; + case ERR_INVALID_REGISTER: + strcpy(out, "InvalidRegister"); + break; + case ERR_UNAUTHORIZED_ACCESS: + strcpy(out, "UnauthorizedAccess"); + break; + case ERR_WATCHDOG_RESET: + strcpy(out, "WatchdogReset"); + break; + case ERR_OUTPUT_BUFFER_OVERFLOW: + strcpy(out, "OutputBufferOverflow"); + break; + case ERR_INSUFFICIENT_BAUD_RATE: + strcpy(out, "InsufficientBaudRate"); + break; + case ERR_ERROR_BUFFER_OVERFLOW: + strcpy(out, "ErrorBufferOverflow"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void BinaryOutputRegister_initialize( + BinaryOutputRegister *reg, + AsyncMode asyncMode, + uint32_t rateDivisor, + CommonGroup commonField, + TimeGroup timeField, + ImuGroup imuField, + GpsGroup gpsField, + AttitudeGroup attitudeField, + InsGroup insField, + GpsGroup gps2Field) +{ + reg->asyncMode = asyncMode; + reg->rateDivisor = (uint16_t)rateDivisor; + reg->commonField = commonField; + reg->timeField = timeField; + reg->imuField = imuField; + reg->gpsField = gpsField; + reg->attitudeField = attitudeField; + reg->insField = insField; + reg->gps2Field = gps2Field; +} + +VnError VnSensor_initialize(VnSensor* s) +{ + VnSerialPort_initialize(&s->serialPort); + s->sendErrorDetectionMode = VNERRORDETECTIONMODE_CHECKSUM; + s->responseTimeoutMs = DEFAULT_RESPONSE_TIMEOUT_MS; + s->retransmitDelayMs = DEFAULT_RETRANSMIT_DELAY_MS; + VnCriticalSection_initialize(&s->transactionCS); + s->responseWaitingForProcessing = false; + s->waitingForResponse = false; + s->bootloaderFilter = false; + VnEvent_initialize(&s->newResponsesEvent); + s->responseLength = 0; + s->runningDataIndex = 0; + VnUartPacketFinder_initialize(&s->packetFinder); + s->asyncPacketFoundHandler = NULL; + s->asyncPacketFoundHandlerUserData = NULL; + s->errorMessageReceivedHandler = NULL; + s->errorMessageReceivedHandlerUserData = NULL; + + VnUartPacketFinder_registerPacketFoundHandler(&s->packetFinder, VnSensor_packetFoundHandler, s); + + return E_NONE; +} + +VnError VnSensor_connect(VnSensor *s, const char *portName, uint32_t baudrate) +{ + VnError error; + + if (VnSensor_isConnected(s)) + return E_INVALID_OPERATION; + + if ((error = VnSerialPort_open(&s->serialPort, portName, baudrate)) != E_NONE) + return error; + + VnSerialPort_registerDataReceivedHandler(&s->serialPort, VnSensor_dataReceivedHandler, s); + + return E_NONE; +} + +VnError VnSensor_disconnect(VnSensor* s) +{ + VnError error; + + if (!VnSensor_isConnected(s)) + return E_INVALID_OPERATION; + + VnSerialPort_unregisterDataReceivedHandler(&s->serialPort); + + if ((error = VnSerialPort_close(&s->serialPort)) != E_NONE) + return error; + + return E_NONE; +} + +VnError VnSensor_changeBaudrate(VnSensor* s, uint32_t baudrate) +{ + VnError error; + + if ((error = VnSensor_writeSerialBaudRate(s, baudrate, true)) != E_NONE) + return error; + + return VnSerialPort_changeBaudrate(&s->serialPort, baudrate); +} + +VnError VnSensor_transaction(VnSensor* s, char* toSend, size_t toSendLength, char* response, size_t* responseLength) +{ + char buffer[COMMAND_MAX_LENGTH]; + size_t finalLength = toSendLength; + + /* Copy over what was provided. */ + memcpy(buffer, toSend, toSendLength); + + /* Add null termination for string operations below. */ + buffer[toSendLength] = '\0'; + + /* First see if an '*' is present. */ + if (strchr(buffer, '*') == NULL) + { + VnUartPacket_finalizeCommand(s->sendErrorDetectionMode, (uint8_t*)buffer, &finalLength); + } + else if (buffer[finalLength - 2] != '\r' && buffer[finalLength - 1] != '\n') + { + buffer[finalLength++] = '\r'; + buffer[finalLength++] = '\n'; + } + + return VnSensor_transactionNoFinalize(s, buffer, finalLength, true, response, responseLength); +} + +bool VnSensor_isConnected(VnSensor* s) +{ + return VnSerialPort_isOpen(&s->serialPort); +} + +VnError VnSensor_writeSettings(VnSensor *s, bool waitForReply) +{ + VnError error; + char toSend[37]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genCmdWriteSettings( + (uint8_t*)toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength)) != E_NONE) + { + return error; + } + + return VnSensor_transactionNoFinalizeWithTiming(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength, 2500, 1000); +} + +VnError VnSensor_restoreFactorySettings(VnSensor *s, bool waitForReply) +{ + VnError error; + char toSend[37]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genCmdRestoreFactorySettings( + (uint8_t*)toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength)) != E_NONE) + { + return error; + } + + return VnSensor_transactionNoFinalizeWithTiming(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength, 2500, 1000); +} + +VnError VnSensor_reset(VnSensor *s, bool waitForReply) +{ + VnError error; + char toSend[37]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genCmdReset( + (uint8_t*)toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength)) != E_NONE) + { + return error; + } + + return VnSensor_transactionNoFinalizeWithTiming(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength, 2500, 1000); + +} + +VnError VnSensor_firmwareUpdateMode(VnSensor* s, bool waitForReply) +{ + VnError error = E_NONE; + char toSend[37]; + size_t toSendLength = sizeof(toSend); + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + memset(toSend, 0, toSendLength); + memset(responseBuffer, 0, responseLength); + + if ((error = VnUartPacket_genCmdFirmwareUpdate((uint8_t*)toSend, sizeof(toSend), s->sendErrorDetectionMode, &toSendLength)) != E_NONE) return error; + + if ((error = VnSensor_transactionNoFinalizeWithTiming(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength, 2500, 1000)) != E_NONE) return error; + + return error; + +} + +VnError VnSensor_tare(VnSensor *s, bool waitForReply) +{ + VnError error; + char toSend[14]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genCmdTare( + (uint8_t*)toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength)) != E_NONE) + { + return error; + } + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_setGyroBias(VnSensor *s, bool waitForReply) +{ + VnError error; + char toSend[14]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genCmdSetGyroBias( + (uint8_t*)toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength)) != E_NONE) + { + return error; + } + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_magneticDisturbancePresent(VnSensor *s, bool disturbancePresent, bool waitForReply) +{ + VnError error; + char toSend[14]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genCmdKnownMagneticDisturbance( + (uint8_t*)toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + disturbancePresent, + &toSendLength)) != E_NONE) + { + return error; + } + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_accelerationDisturbancePresent(VnSensor *s, bool disturbancePresent, bool waitForReply) +{ + VnError error; + char toSend[14]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genCmdKnownAccelerationDisturbance( + (uint8_t*)toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + disturbancePresent, + &toSendLength)) != E_NONE) + { + return error; + } + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +bool VnSensor_verifySensorConnectivity(VnSensor* sensor) +{ + char temp[50]; + + return VnSensor_readModelNumber(sensor, temp, sizeof(temp)) == E_NONE; +} + +uint16_t VnSensor_getResponseTimeoutMs(VnSensor* sensor) +{ + return sensor->responseTimeoutMs; +} + +VnError VnSensor_setResponseTimeoutMs(VnSensor* sensor, uint16_t reponseTimeoutMs) +{ + sensor->responseTimeoutMs = reponseTimeoutMs; + + return E_NONE; +} + +uint16_t VnSensor_getRetransmitDelayMs(VnSensor* sensor) +{ + return sensor->retransmitDelayMs; +} + +VnError VnSensor_setRetransmitDelayMs(VnSensor* sensor, uint16_t retransmitDelayMs) +{ + sensor->retransmitDelayMs = retransmitDelayMs; + + return E_NONE; +} + +VnError VnSensor_registerAsyncPacketReceivedHandler(VnSensor *s, VnSensor_PacketFoundHandler handler, void *userData) +{ + if (s->asyncPacketFoundHandler != NULL) + return E_INVALID_OPERATION; + + s->asyncPacketFoundHandler = handler; + s->asyncPacketFoundHandlerUserData = userData; + + return E_NONE; +} + +VnError VnSensor_unregisterAsyncPacketReceivedHandler(VnSensor *s) +{ + if (s->asyncPacketFoundHandler == NULL) + return E_INVALID_OPERATION; + + s->asyncPacketFoundHandler = NULL; + s->asyncPacketFoundHandlerUserData = NULL; + + return E_NONE; +} + +VnError VnSensor_registerErrorPacketReceivedHandler(VnSensor *s, VnSensor_PacketFoundHandler handler, void *userData) +{ + if (s->errorMessageReceivedHandler != NULL) + return E_INVALID_OPERATION; + + s->errorMessageReceivedHandler = handler; + s->errorMessageReceivedHandlerUserData = userData; + + return E_NONE; +} + +VnError VnSensor_unregisterErrorPacketReceivedHandler(VnSensor *s) +{ + if (s->errorMessageReceivedHandler == NULL) + return E_INVALID_OPERATION; + + s->errorMessageReceivedHandler = NULL; + s->errorMessageReceivedHandlerUserData = NULL; + + return E_NONE; +} + +void VnSensor_dataReceivedHandler(void *userData) +{ + char readBuffer[DEFAULT_READ_BUFFER_SIZE]; + size_t numOfBytesRead = 0; + memset(readBuffer, 0, sizeof(readBuffer)); + + VnSensor *s = (VnSensor*) userData; + + VnSerialPort_read(&s->serialPort, readBuffer, sizeof(readBuffer), &numOfBytesRead); + + if (numOfBytesRead == 0) return; + + VnUartPacketFinder_processData_ex(&s->packetFinder, (uint8_t*)readBuffer, numOfBytesRead, s->bootloaderFilter ); + + s->runningDataIndex += numOfBytesRead; +} + +void VnSensor_onErrorPacketReceived(VnSensor *s, VnUartPacket *packet, size_t runningIndex) +{ + if (s->errorMessageReceivedHandler != NULL) + s->errorMessageReceivedHandler(s->errorMessageReceivedHandlerUserData, packet, runningIndex); +} + +void VnSensor_onAsyncPacketReceived(VnSensor *s, VnUartPacket *packet, size_t runningIndex) +{ + if (s->asyncPacketFoundHandler != NULL) + s->asyncPacketFoundHandler(s->asyncPacketFoundHandlerUserData, packet, runningIndex); +} + +void VnSensor_packetFoundHandler(void *userData, VnUartPacket *packet, size_t runningIndex) +{ + VnSensor *s = (VnSensor*) userData; + + /* Packet should be valid at this point so no need to check its validity. */ + + if (!VnUartPacket_isBootloader(packet)) + { + s->bootloaderFilter = false; + } + + if (VnUartPacket_isError(packet)) + { + if (s->waitingForResponse) + { + VnCriticalSection_enter(&s->transactionCS); + memcpy(s->response, packet->data, packet->length); + s->responseLength = packet->length; + VnEvent_signal(&s->newResponsesEvent); + VnCriticalSection_leave(&s->transactionCS); + } + + VnSensor_onErrorPacketReceived(s, packet, runningIndex); + + return; + } + + if (VnUartPacket_isResponse(packet)) + { + if (s->waitingForResponse) + { + VnCriticalSection_enter(&s->transactionCS); + memcpy(s->response, packet->data, packet->length); + s->responseLength = packet->length; + VnEvent_signal(&s->newResponsesEvent); + VnCriticalSection_leave(&s->transactionCS); + } + + return; + } + + /* This wasn't anything else. We assume it is an async packet. */ + VnSensor_onAsyncPacketReceived(s, packet, runningIndex); +} + +VnError transactionWithWait(VnSensor *s, char* toSend, size_t length, uint16_t responseTimeoutMs, uint16_t retransmitDelayMs, char *responseBuffer, size_t *responseLength) +{ + VnStopwatch timeoutSw; + float curElapsedTime; + VnError waitResult; + bool loopVariable = true; + + /* Make sure we don't have any existing responses. */ + VnCriticalSection_enter(&s->transactionCS); + s->responseWaitingForProcessing = false; + s->waitingForResponse = true; + VnCriticalSection_leave(&s->transactionCS); + + /* Send the command and continue sending if retransmits are enabled until + * we receive the response or timeout. */ + VnStopwatch_initializeAndStart(&timeoutSw); + + VnSerialPort_write(&s->serialPort, toSend, length); + VnStopwatch_elapsedMs(&timeoutSw, &curElapsedTime); + + /* loopVariable is used to avoid a compile variable */ + while (loopVariable) + { + bool shouldRetransmit = false; + + /* Compute how long we should wait for a response before taking more + * action. */ + float responseWaitTime = responseTimeoutMs - curElapsedTime; + if (responseWaitTime > retransmitDelayMs) + { + responseWaitTime = retransmitDelayMs; + shouldRetransmit = true; + } + + /* See if we have time left. */ + if (responseWaitTime < 0) + { + s->waitingForResponse = false; + return E_TIMEOUT; + } + + /* Wait for any new responses that come in or until it is time to send + * a new retransmit. */ + waitResult = VnEvent_waitMs(&s->newResponsesEvent, (uint32_t) responseWaitTime); + + if (waitResult == E_TIMEOUT) + { + if (!shouldRetransmit) + { + s->waitingForResponse = false; + return E_TIMEOUT; + } + } + + if (waitResult == E_SIGNALED) + { + /* Get the current collection of responses. */ + VnCriticalSection_enter(&s->transactionCS); + memcpy(responseBuffer, s->response, s->responseLength); + *responseLength = s->responseLength; + VnCriticalSection_leave(&s->transactionCS); + + /* Process the collection of responses we have. */ + if (VnUartPacket_isErrorRaw((uint8_t*)responseBuffer)) + { + uint8_t sensorError; + + s->waitingForResponse = false; + VnUartPacket_parseErrorRaw((uint8_t*)responseBuffer, &sensorError); + + return (VnError) (sensorError + E_SENSOR_HARD_FAULT - 1); + } + + /* We must have a response packet. */ + s->waitingForResponse = false; + + return E_NONE; + } + + /* Retransmit. */ + VnSerialPort_write(&s->serialPort, toSend, length); + VnStopwatch_elapsedMs(&timeoutSw, &curElapsedTime); + } + + return E_UNKNOWN; +} + +VnError VnSensor_transactionNoFinalizeWithTiming(VnSensor *s, char *toSend, size_t toSendLength, bool waitForReply, char *responseBuffer, size_t* responseBufferSize, uint16_t responseTimeoutMs, uint16_t retransmitDelayMs) +{ + /* This line is used to avoid a compiler warning */ + UNUSED(responseBufferSize); + + if (!VnSensor_isConnected(s)) + return E_INVALID_OPERATION; + + if (waitForReply) + { + size_t responseLength; + + return transactionWithWait(s, toSend, toSendLength, responseTimeoutMs, retransmitDelayMs, responseBuffer, &responseLength); + } + else + { + VnSerialPort_write(&s->serialPort, toSend, toSendLength); + } + + return E_NONE; +} + +VnError VnSensor_transactionNoFinalize(VnSensor *s, char *toSend, size_t toSendLength, bool waitForReply, char *responseBuffer, size_t *responseSize) +{ + return VnSensor_transactionNoFinalizeWithTiming(s, toSend, toSendLength, waitForReply, responseBuffer, responseSize, s->responseTimeoutMs, s->retransmitDelayMs); +} + +VnError VnSensor_readBinaryOutput1(VnSensor *s, BinaryOutputRegister *fields) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + uint16_t asyncMode, outputGroup, commonField, timeField, imuField, gpsField, attitudeField, insField, gps2Field; + + if ((error = VnUartPacket_genReadBinaryOutput1((uint8_t*)toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseBinaryOutputRaw((uint8_t*)responseBuffer, &asyncMode, &fields->rateDivisor, &outputGroup, &commonField, &timeField, &imuField, &gpsField, &attitudeField, &insField, &gps2Field); + + fields->asyncMode = (AsyncMode) asyncMode; + fields->commonField = (CommonGroup) commonField; + fields->timeField = (TimeGroup) timeField; + fields->imuField = (ImuGroup) imuField; + fields->gpsField = (GpsGroup) gpsField; + fields->attitudeField = (AttitudeGroup) attitudeField; + fields->insField = (InsGroup) insField; + fields->gps2Field = (GpsGroup)gps2Field; + + return E_NONE; +} + +VnError VnSensor_writeBinaryOutput1(VnSensor *s, BinaryOutputRegister *fields, bool waitForReply) +{ + char toSend[256]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteBinaryOutput1((uint8_t*)toSend, sizeof(toSend), s->sendErrorDetectionMode, &length, fields->asyncMode, fields->rateDivisor, fields->commonField, fields->timeField, fields->imuField, fields->gpsField, fields->attitudeField, fields->insField, fields->gps2Field)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, length, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readBinaryOutput2(VnSensor *s, BinaryOutputRegister *fields) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + uint16_t asyncMode, outputGroup, commonField, timeField, imuField, gpsField, attitudeField, insField, gps2Field; + + if ((error = VnUartPacket_genReadBinaryOutput2((uint8_t*)toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseBinaryOutputRaw((uint8_t*)responseBuffer, &asyncMode, &fields->rateDivisor, &outputGroup, &commonField, &timeField, &imuField, &gpsField, &attitudeField, &insField, &gps2Field); + + fields->asyncMode = (AsyncMode) asyncMode; + fields->commonField = (CommonGroup) commonField; + fields->timeField = (TimeGroup) timeField; + fields->imuField = (ImuGroup) imuField; + fields->gpsField = (GpsGroup) gpsField; + fields->attitudeField = (AttitudeGroup) attitudeField; + fields->insField = (InsGroup) insField; + fields->gps2Field = (GpsGroup)gps2Field; + + return E_NONE; +} + +VnError VnSensor_writeBinaryOutput2(VnSensor *s, BinaryOutputRegister *fields, bool waitForReply) +{ + char toSend[256]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteBinaryOutput2((uint8_t*)toSend, sizeof(toSend), s->sendErrorDetectionMode, &length, fields->asyncMode, fields->rateDivisor, fields->commonField, fields->timeField, fields->imuField, fields->gpsField, fields->attitudeField, fields->insField, fields->gps2Field)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, length, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readBinaryOutput3(VnSensor *s, BinaryOutputRegister *fields) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + uint16_t asyncMode, outputGroup, commonField, timeField, imuField, gpsField, attitudeField, insField, gps2Field; + + if ((error = VnUartPacket_genReadBinaryOutput3((uint8_t*)toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseBinaryOutputRaw((uint8_t*)responseBuffer, &asyncMode, &fields->rateDivisor, &outputGroup, &commonField, &timeField, &imuField, &gpsField, &attitudeField, &insField, &gps2Field); + + fields->asyncMode = (AsyncMode) asyncMode; + fields->commonField = (CommonGroup) commonField; + fields->timeField = (TimeGroup) timeField; + fields->imuField = (ImuGroup) imuField; + fields->gpsField = (GpsGroup) gpsField; + fields->attitudeField = (AttitudeGroup) attitudeField; + fields->insField = (InsGroup) insField; + fields->gps2Field = (GpsGroup)gps2Field; + + return E_NONE; +} + +VnError VnSensor_writeBinaryOutput3(VnSensor *s, BinaryOutputRegister *fields, bool waitForReply) +{ + char toSend[256]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteBinaryOutput3((uint8_t*)toSend, sizeof(toSend), s->sendErrorDetectionMode, &length, fields->asyncMode, fields->rateDivisor, fields->commonField, fields->timeField, fields->imuField, fields->gpsField, fields->attitudeField, fields->insField, fields->gps2Field)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, length, waitForReply, responseBuffer, &responseLength); +} + +#ifdef EXTRA + +VnError VnSensor_readBinaryOutput4(VnSensor *s, BinaryOutputRegister *fields) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + uint16_t asyncMode, outputGroup, commonField, timeField, imuField, gpsField, attitudeField, insField, gps2Field; + + if ((error = VnUartPacket_genReadBinaryOutput4(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseBinaryOutputRaw((uint8_t*)responseBuffer, &asyncMode, &fields->rateDivisor, &outputGroup, &commonField, &timeField, &imuField, &gpsField, &attitudeField, &insField, &gps2Field); + + fields->asyncMode = (AsyncMode) asyncMode; + fields->commonField = (CommonGroup) commonField; + fields->timeField = (TimeGroup) timeField; + fields->imuField = (ImuGroup) imuField; + fields->gpsField = (GpsGroup) gpsField; + fields->attitudeField = (AttitudeGroup) attitudeField; + fields->insField = (InsGroup) insField; + fields->gps2Field = (GpsGroup)gps2Field; + + return E_NONE; +} + +VnError VnSensor_writeBinaryOutput4(VnSensor *s, BinaryOutputRegister *fields, bool waitForReply) +{ + char toSend[256]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteBinaryOutput4(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length, fields->asyncMode, fields->rateDivisor, fields->commonField, fields->timeField, fields->imuField, fields->gpsField, fields->attitudeField, fields->insField, fields->gps2Field)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, length, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readBinaryOutput5(VnSensor *s, BinaryOutputRegister *fields) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + uint16_t asyncMode, outputGroup, commonField, timeField, imuField, gpsField, attitudeField, insField, gps2Field; + + if ((error = VnUartPacket_genReadBinaryOutput5(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseBinaryOutputRaw((uint8_t*)responseBuffer, &asyncMode, &fields->rateDivisor, &outputGroup, &commonField, &timeField, &imuField, &gpsField, &attitudeField, &insField, &gps2Field); + + fields->asyncMode = (AsyncMode) asyncMode; + fields->commonField = (CommonGroup) commonField; + fields->timeField = (TimeGroup) timeField; + fields->imuField = (ImuGroup) imuField; + fields->gpsField = (GpsGroup) gpsField; + fields->attitudeField = (AttitudeGroup) attitudeField; + fields->insField = (InsGroup) insField; + fields->gps2Field = (GpsGroup)gps2Field; + + return E_NONE; +} + +VnError VnSensor_writeBinaryOutput5(VnSensor *s, BinaryOutputRegister *fields, bool waitForReply) +{ + char toSend[256]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteBinaryOutput5(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length, fields->asyncMode, fields->rateDivisor, fields->commonField, fields->timeField, fields->imuField, fields->gpsField, fields->attitudeField, fields->insField, fields->gps2Field)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, length, waitForReply, responseBuffer, &responseLength); +} + +#endif + +VnError VnSensor_readUserTag(VnSensor *s, char *tagBuffer, size_t tagBufferLength) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + /*Put this here to avoid compiler warnings.*/ + UNUSED(tagBufferLength); + + if ((error = VnUartPacket_genReadUserTag(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseUserTagRaw(responseBuffer, tagBuffer); + + return E_NONE; +} + +VnError VnSensor_writeUserTag(VnSensor *s, char* tag, bool waitForReply) +{ + VnError error; + char toSend[37]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteUserTag( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + tag)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readModelNumber(VnSensor *s, char *productNameBuffer, size_t productNameBufferLength) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + /*Put this here to avoid compiler warnings.*/ + UNUSED(productNameBufferLength); + + if ((error = VnUartPacket_genReadModelNumber(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseModelNumberRaw(responseBuffer, productNameBuffer); + + return E_NONE; +} + +VnError VnSensor_readHardwareRevision(VnSensor *s, uint32_t *revision) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadHardwareRevision(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseHardwareRevisionRaw(responseBuffer, revision); + + return E_NONE; +} + +VnError VnSensor_readSerialNumber(VnSensor *s, uint32_t *serialNum) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadSerialNumber(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseSerialNumberRaw(responseBuffer, serialNum); + + return E_NONE; +} + +VnError VnSensor_readFirmwareVersion(VnSensor *s, char *firmwareVersionBuffer, size_t firmwareVersionBufferLength) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + /*Put this here to avoid compiler warnings.*/ + UNUSED(firmwareVersionBufferLength); + + if ((error = VnUartPacket_genReadFirmwareVersion(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseFirmwareVersionRaw(responseBuffer, firmwareVersionBuffer); + + return E_NONE; +} + +VnError VnSensor_readSerialBaudRate(VnSensor *s, uint32_t *baudrate) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadSerialBaudRate(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseSerialBaudRateRaw(responseBuffer, baudrate); + + return E_NONE; +} + +VnError VnSensor_writeSerialBaudRate(VnSensor *s, uint32_t baudrate, bool waitForReply) +{ + VnError error; + char toSend[25]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteSerialBaudRate( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + baudrate)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readAsyncDataOutputType(VnSensor *s, VnAsciiAsync *ador) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + uint32_t adorPlaceholder; + + if ((error = VnUartPacket_genReadAsyncDataOutputType(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseAsyncDataOutputTypeRaw(responseBuffer, &adorPlaceholder); + + *ador = (VnAsciiAsync) adorPlaceholder; + + return E_NONE; +} + +VnError VnSensor_writeAsyncDataOutputType(VnSensor *s, VnAsciiAsync ador, bool waitForReply) +{ + VnError error; + char toSend[19]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteAsyncDataOutputType( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + ador)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readAsyncDataOutputFrequency(VnSensor *s, uint32_t *adof) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadAsyncDataOutputFrequency(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseAsyncDataOutputFrequencyRaw(responseBuffer, adof); + + return E_NONE; +} + +VnError VnSensor_writeAsyncDataOutputFrequency(VnSensor *s, uint32_t adof, bool waitForReply) +{ + VnError error; + char toSend[26]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteAsyncDataOutputFrequency( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + adof)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readYawPitchRoll(VnSensor *s, vec3f *yawPitchRoll) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadYawPitchRoll(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseYawPitchRollRaw(responseBuffer, yawPitchRoll); + + return E_NONE; +} + +VnError VnSensor_readAttitudeQuaternion(VnSensor *s, vec4f *quat) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadAttitudeQuaternion(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseAttitudeQuaternionRaw(responseBuffer, quat); + + return E_NONE; +} + +VnError VnSensor_readQuaternionMagneticAccelerationAndAngularRates(VnSensor *s, QuaternionMagneticAccelerationAndAngularRatesRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadQuaternionMagneticAccelerationAndAngularRates(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseQuaternionMagneticAccelerationAndAngularRatesRaw(responseBuffer, ®->quat, ®->mag, ®->accel, ®->gyro); + + + return E_NONE; +} + +VnError VnSensor_readMagneticMeasurements(VnSensor *s, vec3f *mag) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadMagneticMeasurements(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseMagneticMeasurementsRaw(responseBuffer, mag); + + return E_NONE; +} + +VnError VnSensor_readAccelerationMeasurements(VnSensor *s, vec3f *accel) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadAccelerationMeasurements(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseAccelerationMeasurementsRaw(responseBuffer, accel); + + return E_NONE; +} + +VnError VnSensor_readAngularRateMeasurements(VnSensor *s, vec3f *gyro) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadAngularRateMeasurements(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseAngularRateMeasurementsRaw(responseBuffer, gyro); + + return E_NONE; +} + +VnError VnSensor_readMagneticAccelerationAndAngularRates(VnSensor *s, MagneticAccelerationAndAngularRatesRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadMagneticAccelerationAndAngularRates(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseMagneticAccelerationAndAngularRatesRaw(responseBuffer, ®->mag, ®->accel, ®->gyro); + + + return E_NONE; +} + +VnError VnSensor_readMagneticAndGravityReferenceVectors(VnSensor *s, MagneticAndGravityReferenceVectorsRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadMagneticAndGravityReferenceVectors(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseMagneticAndGravityReferenceVectorsRaw(responseBuffer, ®->magRef, ®->accRef); + + + return E_NONE; +} + +VnError VnSensor_writeMagneticAndGravityReferenceVectors(VnSensor *s, MagneticAndGravityReferenceVectorsRegister fields, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteMagneticAndGravityReferenceVectors( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + fields.magRef, + fields.accRef)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readMagnetometerCompensation(VnSensor *s, MagnetometerCompensationRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadMagnetometerCompensation(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseMagnetometerCompensationRaw(responseBuffer, ®->c, ®->b); + + + return E_NONE; +} + +VnError VnSensor_writeMagnetometerCompensation(VnSensor *s, MagnetometerCompensationRegister fields, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteMagnetometerCompensation( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + fields.c, + fields.b)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readAccelerationCompensation(VnSensor *s, AccelerationCompensationRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadAccelerationCompensation(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseAccelerationCompensationRaw(responseBuffer, ®->c, ®->b); + + + return E_NONE; +} + +VnError VnSensor_writeAccelerationCompensation(VnSensor *s, AccelerationCompensationRegister fields, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteAccelerationCompensation( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + fields.c, + fields.b)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readReferenceFrameRotation(VnSensor *s, mat3f *c) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadReferenceFrameRotation(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseReferenceFrameRotationRaw(responseBuffer, c); + + return E_NONE; +} + +VnError VnSensor_writeReferenceFrameRotation(VnSensor *s, mat3f c, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteReferenceFrameRotation( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + c)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readYawPitchRollMagneticAccelerationAndAngularRates(VnSensor *s, YawPitchRollMagneticAccelerationAndAngularRatesRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadYawPitchRollMagneticAccelerationAndAngularRates(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseYawPitchRollMagneticAccelerationAndAngularRatesRaw(responseBuffer, ®->yawPitchRoll, ®->mag, ®->accel, ®->gyro); + + + return E_NONE; +} + +VnError VnSensor_readCommunicationProtocolControl(VnSensor *s, CommunicationProtocolControlRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + uint8_t serialCountPlaceholder; + uint8_t serialStatusPlaceholder; + uint8_t spiCountPlaceholder; + uint8_t spiStatusPlaceholder; + uint8_t serialChecksumPlaceholder; + uint8_t spiChecksumPlaceholder; + uint8_t errorModePlaceholder; + + if ((error = VnUartPacket_genReadCommunicationProtocolControl(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseCommunicationProtocolControlRaw(responseBuffer, &serialCountPlaceholder, &serialStatusPlaceholder, &spiCountPlaceholder, &spiStatusPlaceholder, &serialChecksumPlaceholder, &spiChecksumPlaceholder, &errorModePlaceholder); + + reg->serialCount = (VnCountMode) serialCountPlaceholder; + reg->serialStatus = (VnStatusMode) serialStatusPlaceholder; + reg->spiCount = (VnCountMode) spiCountPlaceholder; + reg->spiStatus = (VnStatusMode) spiStatusPlaceholder; + reg->serialChecksum = (VnChecksumMode) serialChecksumPlaceholder; + reg->spiChecksum = (VnChecksumMode) spiChecksumPlaceholder; + reg->errorMode = (VnErrorMode) errorModePlaceholder; + + return E_NONE; +} + +VnError VnSensor_writeCommunicationProtocolControl(VnSensor *s, CommunicationProtocolControlRegister fields, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteCommunicationProtocolControl( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + fields.serialCount, + fields.serialStatus, + fields.spiCount, + fields.spiStatus, + fields.serialChecksum, + fields.spiChecksum, + fields.errorMode)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readSynchronizationControl(VnSensor *s, SynchronizationControlRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + uint8_t syncInModePlaceholder; + uint8_t syncInEdgePlaceholder; + uint32_t reserved1Placeholder; + uint8_t syncOutModePlaceholder; + uint8_t syncOutPolarityPlaceholder; + uint32_t reserved2Placeholder; + + if ((error = VnUartPacket_genReadSynchronizationControl(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseSynchronizationControlRaw(responseBuffer, &syncInModePlaceholder, &syncInEdgePlaceholder, ®->syncInSkipFactor, &reserved1Placeholder, &syncOutModePlaceholder, &syncOutPolarityPlaceholder, ®->syncOutSkipFactor, ®->syncOutPulseWidth, &reserved2Placeholder); + + reg->syncInMode = (VnSyncInMode) syncInModePlaceholder; + reg->syncInEdge = (VnSyncInEdge) syncInEdgePlaceholder; + reg->syncOutMode = (VnSyncOutMode) syncOutModePlaceholder; + reg->syncOutPolarity = (VnSyncOutPolarity) syncOutPolarityPlaceholder; + + return E_NONE; +} + +VnError VnSensor_writeSynchronizationControl(VnSensor *s, SynchronizationControlRegister fields, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteSynchronizationControl( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + fields.syncInMode, + fields.syncInEdge, + fields.syncInSkipFactor, + 0, + fields.syncOutMode, + fields.syncOutPolarity, + fields.syncOutSkipFactor, + fields.syncOutPulseWidth, + 0)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readSynchronizationStatus(VnSensor *s, SynchronizationStatusRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadSynchronizationStatus(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseSynchronizationStatusRaw(responseBuffer, ®->syncInCount, ®->syncInTime, ®->syncOutCount); + + + return E_NONE; +} + +VnError VnSensor_writeSynchronizationStatus(VnSensor *s, SynchronizationStatusRegister fields, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteSynchronizationStatus( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + fields.syncInCount, + fields.syncInTime, + fields.syncOutCount)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readVpeBasicControl(VnSensor *s, VpeBasicControlRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + uint8_t enablePlaceholder; + uint8_t headingModePlaceholder; + uint8_t filteringModePlaceholder; + uint8_t tuningModePlaceholder; + + if ((error = VnUartPacket_genReadVpeBasicControl(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseVpeBasicControlRaw(responseBuffer, &enablePlaceholder, &headingModePlaceholder, &filteringModePlaceholder, &tuningModePlaceholder); + + reg->enable = (VnVpeEnable) enablePlaceholder; + reg->headingMode = (VnHeadingMode) headingModePlaceholder; + reg->filteringMode = (VnVpeMode) filteringModePlaceholder; + reg->tuningMode = (VnVpeMode) tuningModePlaceholder; + + return E_NONE; +} + +VnError VnSensor_writeVpeBasicControl(VnSensor *s, VpeBasicControlRegister fields, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteVpeBasicControl( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + fields.enable, + fields.headingMode, + fields.filteringMode, + fields.tuningMode)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readVpeMagnetometerBasicTuning(VnSensor *s, VpeMagnetometerBasicTuningRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadVpeMagnetometerBasicTuning(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseVpeMagnetometerBasicTuningRaw(responseBuffer, ®->baseTuning, ®->adaptiveTuning, ®->adaptiveFiltering); + + + return E_NONE; +} + +VnError VnSensor_writeVpeMagnetometerBasicTuning(VnSensor *s, VpeMagnetometerBasicTuningRegister fields, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteVpeMagnetometerBasicTuning( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + fields.baseTuning, + fields.adaptiveTuning, + fields.adaptiveFiltering)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readVpeAccelerometerBasicTuning(VnSensor *s, VpeAccelerometerBasicTuningRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadVpeAccelerometerBasicTuning(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseVpeAccelerometerBasicTuningRaw(responseBuffer, ®->baseTuning, ®->adaptiveTuning, ®->adaptiveFiltering); + + + return E_NONE; +} + +VnError VnSensor_writeVpeAccelerometerBasicTuning(VnSensor *s, VpeAccelerometerBasicTuningRegister fields, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteVpeAccelerometerBasicTuning( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + fields.baseTuning, + fields.adaptiveTuning, + fields.adaptiveFiltering)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readMagnetometerCalibrationControl(VnSensor *s, MagnetometerCalibrationControlRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + uint8_t hsiModePlaceholder; + uint8_t hsiOutputPlaceholder; + + if ((error = VnUartPacket_genReadMagnetometerCalibrationControl(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseMagnetometerCalibrationControlRaw(responseBuffer, &hsiModePlaceholder, &hsiOutputPlaceholder, ®->convergeRate); + + reg->hsiMode = (VnHsiMode) hsiModePlaceholder; + reg->hsiOutput = (VnHsiOutput) hsiOutputPlaceholder; + + return E_NONE; +} + +VnError VnSensor_writeMagnetometerCalibrationControl(VnSensor *s, MagnetometerCalibrationControlRegister fields, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteMagnetometerCalibrationControl( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + fields.hsiMode, + fields.hsiOutput, + fields.convergeRate)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readCalculatedMagnetometerCalibration(VnSensor *s, CalculatedMagnetometerCalibrationRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadCalculatedMagnetometerCalibration(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseCalculatedMagnetometerCalibrationRaw(responseBuffer, ®->c, ®->b); + + + return E_NONE; +} + +VnError VnSensor_readVelocityCompensationMeasurement(VnSensor *s, vec3f *velocity) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadVelocityCompensationMeasurement(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseVelocityCompensationMeasurementRaw(responseBuffer, velocity); + + return E_NONE; +} + +VnError VnSensor_writeVelocityCompensationMeasurement(VnSensor *s, vec3f velocity, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteVelocityCompensationMeasurement( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + velocity)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readVelocityCompensationControl(VnSensor *s, VelocityCompensationControlRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + uint8_t modePlaceholder; + + if ((error = VnUartPacket_genReadVelocityCompensationControl(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseVelocityCompensationControlRaw(responseBuffer, &modePlaceholder, ®->velocityTuning, ®->rateTuning); + + reg->mode = (VnVelocityCompensationMode) modePlaceholder; + + return E_NONE; +} + +VnError VnSensor_writeVelocityCompensationControl(VnSensor *s, VelocityCompensationControlRegister fields, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteVelocityCompensationControl( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + fields.mode, + fields.velocityTuning, + fields.rateTuning)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readImuMeasurements(VnSensor *s, ImuMeasurementsRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadImuMeasurements(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseImuMeasurementsRaw(responseBuffer, ®->mag, ®->accel, ®->gyro, ®->temp, ®->pressure); + + + return E_NONE; +} + +VnError VnSensor_readGpsConfiguration(VnSensor *s, GpsConfigurationRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + uint8_t modePlaceholder; + uint8_t ppsSourcePlaceholder; + uint8_t reserved1Placeholder; + uint8_t reserved2Placeholder; + uint8_t reserved3Placeholder; + + if ((error = VnUartPacket_genReadGpsConfiguration(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseGpsConfigurationRaw(responseBuffer, &modePlaceholder, &ppsSourcePlaceholder, &reserved1Placeholder, &reserved2Placeholder, &reserved3Placeholder); + + reg->mode = (VnGpsMode) modePlaceholder; + reg->ppsSource = (VnPpsSource) ppsSourcePlaceholder; + + return E_NONE; +} + +VnError VnSensor_writeGpsConfiguration(VnSensor *s, GpsConfigurationRegister fields, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteGpsConfiguration( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + fields.mode, + fields.ppsSource, + 5, + 0, + 0)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readGpsAntennaOffset(VnSensor *s, vec3f *position) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadGpsAntennaOffset(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseGpsAntennaOffsetRaw(responseBuffer, position); + + return E_NONE; +} + +VnError VnSensor_writeGpsAntennaOffset(VnSensor *s, vec3f position, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteGpsAntennaOffset( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + position)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readGpsSolutionLla(VnSensor *s, GpsSolutionLlaRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + uint8_t gpsFixPlaceholder; + + if ((error = VnUartPacket_genReadGpsSolutionLla(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseGpsSolutionLlaRaw(responseBuffer, ®->time, ®->week, &gpsFixPlaceholder, ®->numSats, ®->lla, ®->nedVel, ®->nedAcc, ®->speedAcc, ®->timeAcc); + + reg->gpsFix = (VnGpsFix) gpsFixPlaceholder; + + return E_NONE; +} + +VnError VnSensor_readGpsSolutionEcef(VnSensor *s, GpsSolutionEcefRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + uint8_t gpsFixPlaceholder; + + if ((error = VnUartPacket_genReadGpsSolutionEcef(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseGpsSolutionEcefRaw(responseBuffer, ®->tow, ®->week, &gpsFixPlaceholder, ®->numSats, ®->position, ®->velocity, ®->posAcc, ®->speedAcc, ®->timeAcc); + + reg->gpsFix = (VnGpsFix) gpsFixPlaceholder; + + return E_NONE; +} + +VnError VnSensor_readInsSolutionLla(VnSensor *s, InsSolutionLlaRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadInsSolutionLla(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseInsSolutionLlaRaw(responseBuffer, ®->time, ®->week, ®->status, ®->yawPitchRoll, ®->position, ®->nedVel, ®->attUncertainty, ®->posUncertainty, ®->velUncertainty); + + + return E_NONE; +} + +VnError VnSensor_readInsSolutionEcef(VnSensor *s, InsSolutionEcefRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadInsSolutionEcef(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseInsSolutionEcefRaw(responseBuffer, ®->time, ®->week, ®->status, ®->yawPitchRoll, ®->position, ®->velocity, ®->attUncertainty, ®->posUncertainty, ®->velUncertainty); + + + return E_NONE; +} + +VnError VnSensor_readInsBasicConfigurationVn200(VnSensor *s, InsBasicConfigurationRegisterVn200 *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + uint8_t scenarioPlaceholder; + uint8_t resv1Placeholder; + uint8_t resv2Placeholder; + + if ((error = VnUartPacket_genReadInsBasicConfiguration(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseInsBasicConfigurationRaw(responseBuffer, &scenarioPlaceholder, ®->ahrsAiding, &resv1Placeholder, &resv2Placeholder); + + reg->scenario = (VnScenario) scenarioPlaceholder; + + return E_NONE; +} + +VnError VnSensor_writeInsBasicConfigurationVn200(VnSensor *s, InsBasicConfigurationRegisterVn200 fields, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteInsBasicConfiguration( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + fields.scenario, + fields.ahrsAiding, + 0, + 0)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readInsBasicConfigurationVn300(VnSensor *s, InsBasicConfigurationRegisterVn300 *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + uint8_t scenarioPlaceholder; + uint8_t resv2Placeholder; + + if ((error = VnUartPacket_genReadInsBasicConfiguration(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseInsBasicConfigurationRaw(responseBuffer, &scenarioPlaceholder, ®->ahrsAiding, ®->estBaseline, &resv2Placeholder); + + reg->scenario = (VnScenario) scenarioPlaceholder; + + return E_NONE; +} + +VnError VnSensor_writeInsBasicConfigurationVn300(VnSensor *s, InsBasicConfigurationRegisterVn300 fields, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteInsBasicConfiguration( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + fields.scenario, + fields.ahrsAiding, + fields.estBaseline, + 0)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readInsStateLla(VnSensor *s, InsStateLlaRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadInsStateLla(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseInsStateLlaRaw(responseBuffer, ®->yawPitchRoll, ®->position, ®->velocity, ®->accel, ®->angularRate); + + + return E_NONE; +} + +VnError VnSensor_readInsStateEcef(VnSensor *s, InsStateEcefRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadInsStateEcef(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseInsStateEcefRaw(responseBuffer, ®->yawPitchRoll, ®->position, ®->velocity, ®->accel, ®->angularRate); + + + return E_NONE; +} + +VnError VnSensor_readStartupFilterBiasEstimate(VnSensor *s, StartupFilterBiasEstimateRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadStartupFilterBiasEstimate(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseStartupFilterBiasEstimateRaw(responseBuffer, ®->gyroBias, ®->accelBias, ®->pressureBias); + + + return E_NONE; +} + +VnError VnSensor_writeStartupFilterBiasEstimate(VnSensor *s, StartupFilterBiasEstimateRegister fields, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteStartupFilterBiasEstimate( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + fields.gyroBias, + fields.accelBias, + fields.pressureBias)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readDeltaThetaAndDeltaVelocity(VnSensor *s, DeltaThetaAndDeltaVelocityRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadDeltaThetaAndDeltaVelocity(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseDeltaThetaAndDeltaVelocityRaw(responseBuffer, ®->deltaTime, ®->deltaTheta, ®->deltaVelocity); + + + return E_NONE; +} + +VnError VnSensor_readDeltaThetaAndDeltaVelocityConfiguration(VnSensor *s, DeltaThetaAndDeltaVelocityConfigurationRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + uint8_t integrationFramePlaceholder; + uint8_t gyroCompensationPlaceholder; + uint8_t accelCompensationPlaceholder; + uint8_t reserved1Placeholder; + uint16_t reserved2Placeholder; + + if ((error = VnUartPacket_genReadDeltaThetaAndDeltaVelocityConfiguration(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseDeltaThetaAndDeltaVelocityConfigurationRaw(responseBuffer, &integrationFramePlaceholder, &gyroCompensationPlaceholder, &accelCompensationPlaceholder, &reserved1Placeholder, &reserved2Placeholder); + + reg->integrationFrame = (VnIntegrationFrame) integrationFramePlaceholder; + reg->gyroCompensation = (VnCompensationMode) gyroCompensationPlaceholder; + reg->accelCompensation = (VnCompensationMode) accelCompensationPlaceholder; + + return E_NONE; +} + +VnError VnSensor_writeDeltaThetaAndDeltaVelocityConfiguration(VnSensor *s, DeltaThetaAndDeltaVelocityConfigurationRegister fields, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteDeltaThetaAndDeltaVelocityConfiguration( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + fields.integrationFrame, + fields.gyroCompensation, + fields.accelCompensation, + 0, + 0)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readReferenceVectorConfiguration(VnSensor *s, ReferenceVectorConfigurationRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + uint8_t resv1Placeholder; + uint8_t resv2Placeholder; + + if ((error = VnUartPacket_genReadReferenceVectorConfiguration(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseReferenceVectorConfigurationRaw(responseBuffer, ®->useMagModel, ®->useGravityModel, &resv1Placeholder, &resv2Placeholder, ®->recalcThreshold, ®->year, ®->position); + + + return E_NONE; +} + +VnError VnSensor_writeReferenceVectorConfiguration(VnSensor *s, ReferenceVectorConfigurationRegister fields, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteReferenceVectorConfiguration( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + fields.useMagModel, + fields.useGravityModel, + 0, + 0, + fields.recalcThreshold, + fields.year, + fields.position)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readGyroCompensation(VnSensor *s, GyroCompensationRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadGyroCompensation(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseGyroCompensationRaw(responseBuffer, ®->c, ®->b); + + + return E_NONE; +} + +VnError VnSensor_writeGyroCompensation(VnSensor *s, GyroCompensationRegister fields, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteGyroCompensation( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + fields.c, + fields.b)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readImuFilteringConfiguration(VnSensor *s, ImuFilteringConfigurationRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + uint8_t magFilterModePlaceholder; + uint8_t accelFilterModePlaceholder; + uint8_t gyroFilterModePlaceholder; + uint8_t tempFilterModePlaceholder; + uint8_t presFilterModePlaceholder; + + if ((error = VnUartPacket_genReadImuFilteringConfiguration(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseImuFilteringConfigurationRaw(responseBuffer, ®->magWindowSize, ®->accelWindowSize, ®->gyroWindowSize, ®->tempWindowSize, ®->presWindowSize, &magFilterModePlaceholder, &accelFilterModePlaceholder, &gyroFilterModePlaceholder, &tempFilterModePlaceholder, &presFilterModePlaceholder); + + reg->magFilterMode = (VnFilterMode) magFilterModePlaceholder; + reg->accelFilterMode = (VnFilterMode) accelFilterModePlaceholder; + reg->gyroFilterMode = (VnFilterMode) gyroFilterModePlaceholder; + reg->tempFilterMode = (VnFilterMode) tempFilterModePlaceholder; + reg->presFilterMode = (VnFilterMode) presFilterModePlaceholder; + + return E_NONE; +} + +VnError VnSensor_writeImuFilteringConfiguration(VnSensor *s, ImuFilteringConfigurationRegister fields, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteImuFilteringConfiguration( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + fields.magWindowSize, + fields.accelWindowSize, + fields.gyroWindowSize, + fields.tempWindowSize, + fields.presWindowSize, + fields.magFilterMode, + fields.accelFilterMode, + fields.gyroFilterMode, + fields.tempFilterMode, + fields.presFilterMode)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readGpsCompassBaseline(VnSensor *s, GpsCompassBaselineRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadGpsCompassBaseline(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseGpsCompassBaselineRaw(responseBuffer, ®->position, ®->uncertainty); + + + return E_NONE; +} + +VnError VnSensor_writeGpsCompassBaseline(VnSensor *s, GpsCompassBaselineRegister fields, bool waitForReply) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genWriteGpsCompassBaseline( + toSend, + sizeof(toSend), + s->sendErrorDetectionMode, + &toSendLength, + fields.position, + fields.uncertainty)) != E_NONE) + return error; + + return VnSensor_transactionNoFinalize(s, toSend, toSendLength, waitForReply, responseBuffer, &responseLength); +} + +VnError VnSensor_readGpsCompassEstimatedBaseline(VnSensor *s, GpsCompassEstimatedBaselineRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + uint8_t resvPlaceholder; + + if ((error = VnUartPacket_genReadGpsCompassEstimatedBaseline(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseGpsCompassEstimatedBaselineRaw(responseBuffer, ®->estBaselineUsed, &resvPlaceholder, ®->numMeas, ®->position, ®->uncertainty); + + + return E_NONE; +} + +VnError VnSensor_readYawPitchRollTrueBodyAccelerationAndAngularRates(VnSensor *s, YawPitchRollTrueBodyAccelerationAndAngularRatesRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadYawPitchRollTrueBodyAccelerationAndAngularRates(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseYawPitchRollTrueBodyAccelerationAndAngularRatesRaw(responseBuffer, ®->yawPitchRoll, ®->bodyAccel, ®->gyro); + + + return E_NONE; +} + +VnError VnSensor_readYawPitchRollTrueInertialAccelerationAndAngularRates(VnSensor *s, YawPitchRollTrueInertialAccelerationAndAngularRatesRegister *reg) +{ + char toSend[17]; + size_t length; + VnError error; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if ((error = VnUartPacket_genReadYawPitchRollTrueInertialAccelerationAndAngularRates(toSend, sizeof(toSend), s->sendErrorDetectionMode, &length)) != E_NONE) + return error; + + if ((error = VnSensor_transactionNoFinalize(s, toSend, length, true, responseBuffer, &responseLength)) != E_NONE) + return error; + + VnUartPacket_parseYawPitchRollTrueInertialAccelerationAndAngularRatesRaw(responseBuffer, ®->yawPitchRoll, ®->inertialAccel, ®->gyro); + + + return E_NONE; +} + +VnError VnSensor_switchProcessors(VnSensor* s, VnProcessorType processor, char *model, char *firmware) +{ + VnError error; + char toSend[256]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + if (strncmp("VN-300", model, 6) == 0) + { + switch (processor) + { + case VNPROCESSOR_GPS: + if ((strstr(model, "SMD") != NULL) || (strstr(model, "DEV") != NULL)) + { + #if VN_HAVE_SECURE_CRT + toSendLength = sprintf_s(toSend, sizeof(toSend), "$VNSPS,1,1,115200"); + #else + toSendLength = sprintf(toSend, "$VNSPS,1,1,115200"); + #endif + } + else if (strstr(model, "CR") != NULL) + { + #if VN_HAVE_SECURE_CRT + toSendLength = sprintf_s(toSend, sizeof(toSend), "$VNDBS,3,1"); + #else + toSendLength = sprintf(toSend, "$VNDBS,3,1"); + #endif + } + break; + } + } + else if ((strncmp("VN-110", model, 6) == 0) || (strncmp("VN-210", model, 6) == 0) || (strncmp("VN-310", model, 6) == 0)) + { + switch (processor) + { + case VNPROCESSOR_NAV: + #if VN_HAVE_SECURE_CRT + toSendLength = sprintf_s(toSend, sizeof(toSend), "$VNSBL,0"); + #else + toSendLength = sprintf(toSend, "$VNSBL,0"); + #endif + break; + case VNPROCESSOR_GPS: + if ((strncmp("VN-310", model, 6) == 0) || (strncmp("VN-210E", model, 7) == 0)) + { + #if VN_HAVE_SECURE_CRT + toSendLength = sprintf_s(toSend, sizeof(toSend), "$VNSBL,1"); + #else + toSendLength = sprintf(toSend, "$VNSBL,1"); + #endif + } + break; + case VNPROCESSOR_IMU: + #if VN_HAVE_SECURE_CRT + toSendLength = sprintf_s(toSend, sizeof(toSend), "$VNSBL,2"); + #else + toSendLength = sprintf(toSend, "$VNSBL,2"); + #endif + break; + } + } + + if (strlen(toSend) > 0) + { + VnUartPacket_finalizeCommand(s->sendErrorDetectionMode, (uint8_t*)toSend, &toSendLength); + error = VnSensor_transactionNoFinalizeWithTiming(s, toSend, toSendLength, true, responseBuffer, &responseLength, 6000, 6000); + + if (error == E_NONE) + { + /* Check the response buffer for errors */ + + /* Wait 2 seconds to allow the processor to switch */ + VnThread_sleepMs(2000); + + } + + return error; + } + else + { + return E_NOT_SUPPORTED; + } +} + +FILE *VnSensor_openFirmwareUpdateFile(char* filename) +{ +#if VN_HAVE_SECURE_CRT + FILE *file; + int error = fopen_s(&file, filename, "r"); + return file; +#else + return fopen(filename, "r"); +#endif + +} + +bool VnSensor_getNextFirmwareUpdateRecord(FILE* file, char* record, int MaxRecordSize) +{ + bool result = false; + + +#if VN_HAVE_SECURE_CRT + int retval = fscanf_s(file, "%s\n", record, MaxRecordSize); +#else + int retval = fscanf(file, "%s\n", record); +#endif + + if (retval != EOF) + { + result = true; + } + + return result; +} + +void VnSensor_closeFirmwareUpdateFile(FILE* file) +{ + if (file != NULL) + { + int error = fclose(file); + file = NULL; + } +} + +void VnSensor_calibrateBootloader(VnSensor *s) +{ + char calibration[] = " "; + char bootloaderVersion[64]; + size_t bootloaderVersionSize = sizeof(bootloaderVersion); + memset(bootloaderVersion, 0, 64); + uint16_t responseTimeoutMs = 500; + uint16_t retransmitDelayMs = responseTimeoutMs; + + s->bootloaderFilter = true; + if (E_NONE == VnSensor_transactionNoFinalizeWithTiming(s, calibration, sizeof(calibration) - 2, true, bootloaderVersion, &bootloaderVersionSize, responseTimeoutMs, retransmitDelayMs)) + { + /* printf("Bootloader: %s\n", bootloaderVersion); */ + } +} + +VnError VnSensor_writeFirmwareUpdateRecord(VnSensor* s, char* record) +{ + VnError error = E_NONE; + char toSend[MAXFIRMWAREUPDATERECORDSIZE +12]; + size_t toSendLength; + char responseBuffer[0x100]; + size_t responseLength = sizeof(responseBuffer); + + memset(responseBuffer, 0, 0x100); + + /* VNERRORDETECTIONMODE_CRC - Must be 16-bit CRC for Firmware Updates, also, strip off the leading ':' from the record */ + if ((error = VnUartPacket_genWriteFirmwareUpdate(toSend, sizeof(toSend), VNERRORDETECTIONMODE_CRC, &toSendLength, &record[1])) != E_NONE) + return error; + +RESEND: + s->bootloaderFilter = true; + error = VnSensor_transactionNoFinalizeWithTiming(s, toSend, toSendLength, true, responseBuffer, &responseLength, 7000, 7000); + if (error != E_NONE) + { + char buffer[128]; + memset(buffer, 0, sizeof(buffer)); + strFromSensorError(buffer, error); + /* printf("VnSensor_writeFirmwareUpdate: Error %d - %s", error, buffer); */ + } + else + { + /* Check Firmware Update Response Code */ + int response = VnUtil_toUint8FromHexStr(&(responseBuffer[7])) + E_BOOTLOADER_NONE; + error = (VnError)response; + + if (response == E_BOOTLOADER_COMM_ERROR) goto RESEND; + else if (response != E_BOOTLOADER_NONE) + { + char buffer[128]; + memset(buffer, 0, sizeof(buffer)); + strFromVnError(buffer, response); + /* printf("Error: %s\n", buffer); */ + } + } + return error; +} + +VnError VnSensor_firmwareUpdate(VnSensor* s, int baudRate, char* filename) +{ + VnError error = E_NONE; + int RecordSize = MAXFIRMWAREUPDATERECORDSIZE; + char record[MAXFIRMWAREUPDATERECORDSIZE]; + bool stillGoing = true; + + /* Open Firmware Update File*/ + FILE* firmwareUpdateFile = VnSensor_openFirmwareUpdateFile(filename); + + /* Enter the bootloader */ + error = VnSensor_firmwareUpdateMode(s, true); + + /* Change the baud rate for updating the firmware */ + error = VnSerialPort_changeBaudrate(&(s->serialPort), baudRate); + + /* Give the processor some time to reboot */ + VnThread_sleepMs(1000); + + /* Calibrate the Bootloader before sending records */ + VnSensor_calibrateBootloader(s); + + /* Write the firmware update file to the sensor, one record at a time */ + while (stillGoing) + { + stillGoing = VnSensor_getNextFirmwareUpdateRecord(firmwareUpdateFile, record, RecordSize); + if (stillGoing) + { + error = VnSensor_writeFirmwareUpdateRecord(s, record); + if (error != E_BOOTLOADER_NONE) + { + stillGoing = false; + } + } + } + + /* Switch the baud rate back to the original setting */ + error = VnSerialPort_changeBaudrate(&(s->serialPort), baudRate); + + /* Close the firmware update file */ + VnSensor_closeFirmwareUpdateFile(firmwareUpdateFile); + + /* Exit Bootloader mode. Just sleep for 10 seconds */ + VnThread_sleepMs(10000); + + /* Do a reset */ + VnSensor_reset(s, true); + + /* Wait 2 seconds for the reset to finish */ + VnThread_sleepMs(2000); + + return error; +} + +void strFromSyncInMode(char *out, VnSyncInMode 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) + { + #ifdef EXTRA + case VNSYNCINMODE_COUNT2: + strcpy(out, "Count2"); + break; + case VNSYNCINMODE_ADC2: + strcpy(out, "Adc2"); + break; + case VNSYNCINMODE_ASYNC2: + strcpy(out, "Async2"); + break; + #endif + case VNSYNCINMODE_COUNT: + strcpy(out, "Count"); + break; + case VNSYNCINMODE_IMU: + strcpy(out, "Imu"); + break; + case VNSYNCINMODE_ASYNC: + strcpy(out, "Async"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromSyncInEdge(char *out, VnSyncInEdge 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 VNSYNCINEDGE_RISING: + strcpy(out, "Rising"); + break; + case VNSYNCINEDGE_FALLING: + strcpy(out, "Falling"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromSyncOutMode(char *out, VnSyncOutMode 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 VNSYNCOUTMODE_NONE: + strcpy(out, "None"); + break; + case VNSYNCOUTMODE_ITEMSTART: + strcpy(out, "ItemStart"); + break; + case VNSYNCOUTMODE_IMUREADY: + strcpy(out, "ImuReady"); + break; + case VNSYNCOUTMODE_INS: + strcpy(out, "Ins"); + break; + case VNSYNCOUTMODE_GPSPPS: + strcpy(out, "GpsPps"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromSyncOutPolarity(char *out, VnSyncOutPolarity 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 VNSYNCOUTPOLARITY_NEGATIVE: + strcpy(out, "Negative"); + break; + case VNSYNCOUTPOLARITY_POSITIVE: + strcpy(out, "Positive"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromCountMode(char *out, VnCountMode 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 VNCOUNTMODE_NONE: + strcpy(out, "None"); + break; + case VNCOUNTMODE_SYNCINCOUNT: + strcpy(out, "SyncInCount"); + break; + case VNCOUNTMODE_SYNCINTIME: + strcpy(out, "SyncInTime"); + break; + case VNCOUNTMODE_SYNCOUTCOUNTER: + strcpy(out, "SyncOutCounter"); + break; + case VNCOUNTMODE_GPSPPS: + strcpy(out, "GpsPps"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromStatusMode(char *out, VnStatusMode 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 VNSTATUSMODE_OFF: + strcpy(out, "Off"); + break; + case VNSTATUSMODE_VPESTATUS: + strcpy(out, "VpeStatus"); + break; + case VNSTATUSMODE_INSSTATUS: + strcpy(out, "InsStatus"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromChecksumMode(char *out, VnChecksumMode 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 VNCHECKSUMMODE_OFF: + strcpy(out, "Off"); + break; + case VNCHECKSUMMODE_CHECKSUM: + strcpy(out, "Checksum"); + break; + case VNCHECKSUMMODE_CRC: + strcpy(out, "Crc"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromErrorMode(char *out, VnErrorMode 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 VNERRORMODE_IGNORE: + strcpy(out, "Ignore"); + break; + case VNERRORMODE_SEND: + strcpy(out, "Send"); + break; + case VNERRORMODE_SENDANDOFF: + strcpy(out, "SendAndOff"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromFilterMode(char *out, VnFilterMode 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 VNFILTERMODE_NOFILTERING: + strcpy(out, "NoFiltering"); + break; + case VNFILTERMODE_ONLYRAW: + strcpy(out, "OnlyRaw"); + break; + case VNFILTERMODE_ONLYCOMPENSATED: + strcpy(out, "OnlyCompensated"); + break; + case VNFILTERMODE_BOTH: + strcpy(out, "Both"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromIntegrationFrame(char *out, VnIntegrationFrame 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 VNINTEGRATIONFRAME_BODY: + strcpy(out, "Body"); + break; + case VNINTEGRATIONFRAME_NED: + strcpy(out, "Ned"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromCompensationMode(char *out, VnCompensationMode 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 VNCOMPENSATIONMODE_NONE: + strcpy(out, "None"); + break; + case VNCOMPENSATIONMODE_BIAS: + strcpy(out, "Bias"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromGpsFix(char *out, VnGpsFix 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 VNGPSFIX_NOFIX: + strcpy(out, "NoFix"); + break; + case VNGPSFIX_TIMEONLY: + strcpy(out, "TimeOnly"); + break; + case VNGPSFIX_2D: + strcpy(out, "2D"); + break; + case VNGPSFIX_3D: + strcpy(out, "3D"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromGpsMode(char *out, VnGpsMode 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 VNGPSMODE_ONBOARDGPS: + strcpy(out, "OnBoardGps"); + break; + case VNGPSMODE_EXTERNALGPS: + strcpy(out, "ExternalGps"); + break; + case VNGPSMODE_EXTERNALVN200GPS: + strcpy(out, "ExternalVn200Gps"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromPpsSource(char *out, VnPpsSource 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 VNPPSSOURCE_GPSPPSRISING: + strcpy(out, "GpsPpsRising"); + break; + case VNPPSSOURCE_GPSPPSFALLING: + strcpy(out, "GpsPpsFalling"); + break; + case VNPPSSOURCE_SYNCINRISING: + strcpy(out, "SyncInRising"); + break; + case VNPPSSOURCE_SYNCINFALLING: + strcpy(out, "SyncInFalling"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromVpeEnable(char *out, VnVpeEnable 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 VNVPEENABLE_DISABLE: + strcpy(out, "Disable"); + break; + case VNVPEENABLE_ENABLE: + strcpy(out, "Enable"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromHeadingMode(char *out, VnHeadingMode 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 VNHEADINGMODE_ABSOLUTE: + strcpy(out, "Absolute"); + break; + case VNHEADINGMODE_RELATIVE: + strcpy(out, "Relative"); + break; + case VNHEADINGMODE_INDOOR: + strcpy(out, "Indoor"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromVpeMode(char *out, VnVpeMode 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 VNVPEMODE_OFF: + strcpy(out, "Off"); + break; + case VNVPEMODE_MODE1: + strcpy(out, "Mode1"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromScenario(char *out, VnScenario 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 VNSCENARIO_AHRS: + strcpy(out, "Ahrs"); + break; + case VNSCENARIO_INSWITHPRESSURE: + strcpy(out, "InsWithPressure"); + break; + case VNSCENARIO_INSWITHOUTPRESSURE: + strcpy(out, "InsWithoutPressure"); + break; + case VNSCENARIO_GPSMOVINGBASELINEDYNAMIC: + strcpy(out, "GpsMovingBaselineDynamic"); + break; + case VNSCENARIO_GPSMOVINGBASELINESTATIC: + strcpy(out, "GpsMovingBaselineStatic"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromHsiMode(char *out, VnHsiMode 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 VNHSIMODE_OFF: + strcpy(out, "Off"); + break; + case VNHSIMODE_RUN: + strcpy(out, "Run"); + break; + case VNHSIMODE_RESET: + strcpy(out, "Reset"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromHsiOutput(char *out, VnHsiOutput 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 VNHSIOUTPUT_NOONBOARD: + strcpy(out, "NoOnboard"); + break; + case VNHSIOUTPUT_USEONBOARD: + strcpy(out, "UseOnboard"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromVelocityCompensationMode(char *out, VnVelocityCompensationMode 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 VNVELOCITYCOMPENSATIONMODE_DISABLED: + strcpy(out, "Disabled"); + break; + case VNVELOCITYCOMPENSATIONMODE_BODYMEASUREMENT: + strcpy(out, "BodyMeasurement"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromMagneticMode(char *out, VnMagneticMode 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 VNMAGNETICMODE_2D: + strcpy(out, "2D"); + break; + case VNMAGNETICMODE_3D: + strcpy(out, "3D"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromExternalSensorMode(char *out, VnExternalSensorMode 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 VNEXTERNALSENSORMODE_INTERNAL: + strcpy(out, "Internal"); + break; + case VNEXTERNALSENSORMODE_EXTERNAL200HZ: + strcpy(out, "External200Hz"); + break; + case VNEXTERNALSENSORMODE_EXTERNALONUPDATE: + strcpy(out, "ExternalOnUpdate"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} + +void strFromFoamInit(char *out, VnFoamInit 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 VNFOAMINIT_NOFOAMINIT: + strcpy(out, "NoFoamInit"); + break; + case VNFOAMINIT_FOAMINITPITCHROLL: + strcpy(out, "FoamInitPitchRoll"); + break; + case VNFOAMINIT_FOAMINITHEADINGPITCHROLL: + strcpy(out, "FoamInitHeadingPitchRoll"); + break; + case VNFOAMINIT_FOAMINITPITCHROLLCOVARIANCE: + strcpy(out, "FoamInitPitchRollCovariance"); + break; + case VNFOAMINIT_FOAMINITHEADINGPITCHROLLCOVARIANCE: + strcpy(out, "FoamInitHeadingPitchRollCovariance"); + break; + default: + strcpy(out, "Unknown"); + break; + } + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif +} diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/sensors/compositedata.c b/src/drivers/ins/vectornav/libvnc/src/vn/sensors/compositedata.c new file mode 100644 index 0000000000..00b735982b --- /dev/null +++ b/src/drivers/ins/vectornav/libvnc/src/vn/sensors/compositedata.c @@ -0,0 +1,763 @@ +#include +#include + +#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); +} diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/sensors/ezasyncdata.c b/src/drivers/ins/vectornav/libvnc/src/vn/sensors/ezasyncdata.c new file mode 100644 index 0000000000..74193c4e43 --- /dev/null +++ b/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 +#include +#include +#include + +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); +} diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/sensors/searcher.c b/src/drivers/ins/vectornav/libvnc/src/vn/sensors/searcher.c new file mode 100644 index 0000000000..3a537acffb --- /dev/null +++ b/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 +#include +#include +#include + + +#ifdef __linux__ +#include +#include +#include + +#elif defined _WIN32 +#include +#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; + } +} diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/util.c b/src/drivers/ins/vectornav/libvnc/src/vn/util.c new file mode 100644 index 0000000000..bb078c0397 --- /dev/null +++ b/src/drivers/ins/vectornav/libvnc/src/vn/util.c @@ -0,0 +1,391 @@ +#include "vn/util.h" + +#include +#include + +#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 +#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; +} diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/xplat/criticalsection.c b/src/drivers/ins/vectornav/libvnc/src/vn/xplat/criticalsection.c new file mode 100644 index 0000000000..a9b7ceaad2 --- /dev/null +++ b/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; +} diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/xplat/event.c b/src/drivers/ins/vectornav/libvnc/src/vn/xplat/event.c new file mode 100644 index 0000000000..59dec30cb9 --- /dev/null +++ b/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 + #include +#endif + +#ifdef __APPLE__ + #include + #include + #include + #include +#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; +} diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/xplat/serialport.c b/src/drivers/ins/vectornav/libvnc/src/vn/xplat/serialport.c new file mode 100644 index 0000000000..79848b660b --- /dev/null +++ b/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 + #include + #include + #include + #include + #include + #include +#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; +} diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/xplat/thread.c b/src/drivers/ins/vectornav/libvnc/src/vn/xplat/thread.c new file mode 100644 index 0000000000..da086b0195 --- /dev/null +++ b/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 + #include + #include + #include +#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 +} diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/xplat/time.c b/src/drivers/ins/vectornav/libvnc/src/vn/xplat/time.c new file mode 100644 index 0000000000..e10ff07ed9 --- /dev/null +++ b/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 +#elif defined __APPLE__ + #include + #include +#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; +} diff --git a/src/drivers/ins/vectornav/module.yaml b/src/drivers/ins/vectornav/module.yaml new file mode 100644 index 0000000000..c95bfeb608 --- /dev/null +++ b/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 + diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 9dfa49c6b5..2a0a2c957c 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/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");