From 63ca3e03b23f08390b2cb47d82182f225963e6bf Mon Sep 17 00:00:00 2001 From: "r.koeppe" Date: Thu, 16 May 2024 01:40:25 +0200 Subject: [PATCH] extracted structs from vnproglib and made them usable for uC --- INS-VN-200/Core/Src/main.cpp | 7 +- vn-interface/include/vn-interface/enum.h | 670 ++++++++++++++ vn-interface/include/vn-interface/helper.h | 140 +++ vn-interface/include/vn-interface/int.h | 29 + .../include/vn-interface/math/matrix.h | 69 ++ .../include/vn-interface/math/vector.h | 146 +++ vn-interface/include/vn-interface/registers.h | 846 ++++++++++++++++++ vn-interface/include/vn-interface/types.h | 56 ++ vn-interface/include/vn-interface/vn200.h | 138 --- vn-interface/src/helper.cpp | 3 + vn-interface/src/math/matrix.c | 20 + vn-interface/src/math/vector.c | 118 +++ vn-interface/src/registers.c | 15 + vn-interface/src/vn200.cpp | 5 - 14 files changed, 2116 insertions(+), 146 deletions(-) create mode 100644 vn-interface/include/vn-interface/enum.h create mode 100644 vn-interface/include/vn-interface/helper.h create mode 100644 vn-interface/include/vn-interface/int.h create mode 100644 vn-interface/include/vn-interface/math/matrix.h create mode 100644 vn-interface/include/vn-interface/math/vector.h create mode 100644 vn-interface/include/vn-interface/registers.h create mode 100644 vn-interface/include/vn-interface/types.h delete mode 100644 vn-interface/include/vn-interface/vn200.h create mode 100644 vn-interface/src/helper.cpp create mode 100644 vn-interface/src/math/matrix.c create mode 100644 vn-interface/src/math/vector.c create mode 100644 vn-interface/src/registers.c delete mode 100644 vn-interface/src/vn200.cpp diff --git a/INS-VN-200/Core/Src/main.cpp b/INS-VN-200/Core/Src/main.cpp index 92e9704..ff1054e 100644 --- a/INS-VN-200/Core/Src/main.cpp +++ b/INS-VN-200/Core/Src/main.cpp @@ -23,7 +23,8 @@ /* USER CODE BEGIN Includes */ #include "Converter/Converter.h" // #include "Converter/vn200.h" -#include +#include +#include /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -100,8 +101,8 @@ int main(void) { uint16_t can_id = 0x12; /*TODO*/ uint8_t data[8] = {0}; /*TODO*/ - VN200::SPI::pkg_request_read_t package_request_read_imu(VN200::SPI::IMU::ID); - VN200::SPI::pkg_response_t package_response_read_imu; + vn::pkg_request_read_t package_request_read_imu(vn::ImuMeasurementsRegisterID); + vn::pkg_response_t package_response_read_imu; /* USER CODE END 2 */ diff --git a/vn-interface/include/vn-interface/enum.h b/vn-interface/include/vn-interface/enum.h new file mode 100644 index 0000000..617d88c --- /dev/null +++ b/vn-interface/include/vn-interface/enum.h @@ -0,0 +1,670 @@ +#ifndef VNENUM_H_INCLUDED +#define VNENUM_H_INCLUDED + +/** \brief The different types of UART packets. */ +typedef enum +{ + PACKETTYPE_BINARY, /**< Binary packet. */ + PACKETTYPE_ASCII, /**< ASCII packet. */ + PACKETTYPE_UNKNOWN /**< Unknown packet type. */ +} PacketType; + +/** \brief The available binary output groups. */ +typedef enum +{ + BINARYGROUPTYPE_COMMON = 0x01, /**< Common group. */ + BINARYGROUPTYPE_TIME = 0x02, /**< Time group. */ + BINARYGROUPTYPE_IMU = 0x04, /**< IMU group. */ + BINARYGROUPTYPE_GPS = 0x08, /**< GPS group. */ + BINARYGROUPTYPE_ATTITUDE = 0x10, /**< Attitude group. */ + BINARYGROUPTYPE_INS = 0x20, /**< INS group. */ + BINARYGROUPTYPE_GPS2 = 0x40 /**< GPS2 group. */ +} BinaryGroupType; + +/** \brief Async modes for the Binary Output registers. */ +typedef enum +{ + ASYNCMODE_NONE = 0, /**< None. */ + ASYNCMODE_PORT1 = 1, /**< Serial port 1. */ + ASYNCMODE_PORT2 = 2, /**< Serial port 2. */ + ASYNCMODE_BOTH = 3 /**< Both serial ports. */ +} AsyncMode; + +/** \brief Flags for the binary group 1 'Common' in the binary output registers. */ +typedef enum +{ + COMMONGROUP_NONE = 0x0000, /**< None. */ + COMMONGROUP_TIMESTARTUP = 0x0001, /**< TimeStartup. */ + COMMONGROUP_TIMEGPS = 0x0002, /**< TimeGps. */ + COMMONGROUP_TIMESYNCIN = 0x0004, /**< TimeSyncIn. */ + COMMONGROUP_YAWPITCHROLL = 0x0008, /**< YawPitchRoll. */ + COMMONGROUP_QUATERNION = 0x0010, /**< Quaternion. */ + COMMONGROUP_ANGULARRATE = 0x0020, /**< AngularRate. */ + COMMONGROUP_POSITION = 0x0040, /**< Position. */ + COMMONGROUP_VELOCITY = 0x0080, /**< Velocity. */ + COMMONGROUP_ACCEL = 0x0100, /**< Accel. */ + COMMONGROUP_IMU = 0x0200, /**< Imu. */ + COMMONGROUP_MAGPRES = 0x0400, /**< MagPres. */ + COMMONGROUP_DELTATHETA = 0x0800, /**< DeltaTheta. */ + COMMONGROUP_INSSTATUS = 0x1000, /**< InsStatus. */ + COMMONGROUP_SYNCINCNT = 0x2000, /**< SyncInCnt. */ + COMMONGROUP_TIMEGPSPPS = 0x4000 /**< TimeGpsPps. */ +} CommonGroup; + +/** \brief Flags for the binary group 2 'Time' in the binary output registers. */ +typedef enum +{ + TIMEGROUP_NONE = 0x0000, /**< None. */ + TIMEGROUP_TIMESTARTUP = 0x0001, /**< TimeStartup. */ + TIMEGROUP_TIMEGPS = 0x0002, /**< TimeGps. */ + TIMEGROUP_GPSTOW = 0x0004, /**< GpsTow. */ + TIMEGROUP_GPSWEEK = 0x0008, /**< GpsWeek. */ + TIMEGROUP_TIMESYNCIN = 0x0010, /**< TimeSyncIn. */ + TIMEGROUP_TIMEGPSPPS = 0x0020, /**< TimeGpsPps. */ + TIMEGROUP_TIMEUTC = 0x0040, /**< TimeUTC. */ + TIMEGROUP_SYNCINCNT = 0x0080, /**< SyncInCnt. */ + TIMEGROUP_SYNCOUTCNT = 0x0100, /**< SyncOutCnt. */ + TIMEGROUP_TIMESTATUS = 0x0200 /**< TimeStatus. */ +} TimeGroup; + +/** \brief Flags for the binary group 3 'IMU' in the binary output registers. */ +typedef enum +{ + IMUGROUP_NONE = 0x0000, /**< None. */ + IMUGROUP_IMUSTATUS = 0x0001, /**< ImuStatus. */ + IMUGROUP_UNCOMPMAG = 0x0002, /**< UncompMag. */ + IMUGROUP_UNCOMPACCEL = 0x0004, /**< UncompAccel. */ + IMUGROUP_UNCOMPGYRO = 0x0008, /**< UncompGyro. */ + IMUGROUP_TEMP = 0x0010, /**< Temp. */ + IMUGROUP_PRES = 0x0020, /**< Pres. */ + IMUGROUP_DELTATHETA = 0x0040, /**< DeltaTheta. */ + IMUGROUP_DELTAVEL = 0x0080, /**< DeltaVel. */ + IMUGROUP_MAG = 0x0100, /**< Mag. */ + IMUGROUP_ACCEL = 0x0200, /**< Accel. */ + IMUGROUP_ANGULARRATE = 0x0400, /**< AngularRate. */ + IMUGROUP_SENSSAT = 0x0800 /**< SensSat. */ + #ifdef EXTRA + , + IMUGROUP_RAW = 0x1000 /**< Raw. */ + #endif +} ImuGroup; + +/** \brief Flags for the binary group 4 'GPS' in the binary output registers. */ +typedef enum +{ + GPSGROUP_NONE = 0x0000, /**< None. */ + GPSGROUP_UTC = 0x0001, /**< UTC. */ + GPSGROUP_TOW = 0x0002, /**< Tow. */ + GPSGROUP_WEEK = 0x0004, /**< Week. */ + GPSGROUP_NUMSATS = 0x0008, /**< NumSats. */ + GPSGROUP_FIX = 0x0010, /**< Fix. */ + GPSGROUP_POSLLA = 0x0020, /**< PosLla. */ + GPSGROUP_POSECEF = 0x0040, /**< PosEcef. */ + GPSGROUP_VELNED = 0x0080, /**< VelNed. */ + GPSGROUP_VELECEF = 0x0100, /**< VelEcef. */ + GPSGROUP_POSU = 0x0200, /**< PosU. */ + GPSGROUP_VELU = 0x0400, /**< VelU. */ + GPSGROUP_TIMEU = 0x0800, /**< TimeU. */ + GPSGROUP_TIMEINFO = 0x1000, /**< TimeInfo. */ + GPSGROUP_DOP = 0x2000 /**< DOP. */ + + #ifdef EXTRA + , + GPSGROUP_SVSTAT = 0x1000 /**< SvStat. */ + #endif +} GpsGroup; + +/** \brief Flags for the binary group 5 'Attitude' in the binary output registers. */ +typedef enum +{ + ATTITUDEGROUP_NONE = 0x0000, /**< None. */ + ATTITUDEGROUP_VPESTATUS = 0x0001, /**< VpeStatus. */ + ATTITUDEGROUP_YAWPITCHROLL = 0x0002, /**< YawPitchRoll. */ + ATTITUDEGROUP_QUATERNION = 0x0004, /**< Quaternion. */ + ATTITUDEGROUP_DCM = 0x0008, /**< DCM. */ + ATTITUDEGROUP_MAGNED = 0x0010, /**< MagNed. */ + ATTITUDEGROUP_ACCELNED = 0x0020, /**< AccelNed. */ + ATTITUDEGROUP_LINEARACCELBODY = 0x0040, /**< LinearAccelBody. */ + ATTITUDEGROUP_LINEARACCELNED = 0x0080, /**< LinearAccelNed. */ + ATTITUDEGROUP_YPRU = 0x0100 /**< YprU. */ + #ifdef EXTRA + , + ATTITUDEGROUP_YPRRATE = 0x0200, /**< YprRate. */ + ATTITUDEGROUP_STATEAHRS = 0x0400, /**< StateAhrs. */ + ATTITUDEGROUP_COVAHRS = 0x0800 /**< CovAhrs. */ + #endif +} AttitudeGroup; + +/** \brief Flags for the binary group 6 'INS' in the binary output registers. */ +typedef enum +{ + INSGROUP_NONE = 0x0000, /**< None. */ + INSGROUP_INSSTATUS = 0x0001, /**< InsStatus. */ + INSGROUP_POSLLA = 0x0002, /**< PosLla. */ + INSGROUP_POSECEF = 0x0004, /**< PosEcef. */ + INSGROUP_VELBODY = 0x0008, /**< VelBody. */ + INSGROUP_VELNED = 0x0010, /**< VelNed. */ + INSGROUP_VELECEF = 0x0020, /**< VelEcef. */ + INSGROUP_MAGECEF = 0x0040, /**< MagEcef. */ + INSGROUP_ACCELECEF = 0x0080, /**< AccelEcef. */ + INSGROUP_LINEARACCELECEF = 0x0100, /**< LinearAccelEcef. */ + INSGROUP_POSU = 0x0200, /**< PosU. */ + INSGROUP_VELU = 0x0400 /**< VelU. */ + #ifdef EXTRA + , + INSGROUP_STATEINS = 0x0800, /**< StateIns. */ + INSGROUP_COVINS = 0x1000 /**< CovIns. */ + #endif +} InsGroup; + +/** \brief Enumeration of the velocity types */ +typedef enum +{ + CDVEL_None, + CDVEL_GpsNed, + CDVEL_GpsEcef, + CDVEL_EstimatedNed, + CDVEL_EstimatedEcef, + CDVEL_EstimatedBody +} VelocityType; + +/** \brief Errors that the VectorNav sensor can report. */ +typedef enum +{ + ERR_HARD_FAULT = 1, /**< Hard fault. */ + ERR_SERIAL_BUFFER_OVERFLOW = 2, /**< Serial buffer overflow. */ + ERR_INVALID_CHECKSUM = 3, /**< Invalid checksum. */ + ERR_INVALID_COMMAND = 4, /**< Invalid command. */ + ERR_NOT_ENOUGH_PARAMETERS = 5, /**< Not enough parameters. */ + ERR_TOO_MANY_PARAMETERS = 6, /**< Too many parameters. */ + ERR_INVALID_PARAMETER = 7, /**< Invalid parameter. */ + ERR_INVALID_REGISTER = 8, /**< Invalid register. */ + ERR_UNAUTHORIZED_ACCESS = 9, /**< Unauthorized access. */ + ERR_WATCHDOG_RESET = 10, /**< Watchdog reset. */ + ERR_OUTPUT_BUFFER_OVERFLOW = 11, /**< Output buffer overflow. */ + ERR_INSUFFICIENT_BAUD_RATE = 12, /**< Insufficient baud rate. */ + ERR_ERROR_BUFFER_OVERFLOW = 255 /**< Error buffer overflow. */ +} SensorError; + +/** \brief Enumeration of the various error messages used by the library. */ +typedef enum +{ + /** Indicates there were no errors encountered. */ + E_NONE, + + /** Indicates an unknown error occurred. */ + E_UNKNOWN, + + /** Indicates a provided buffer was too small to complete the action. */ + E_BUFFER_TOO_SMALL, + + /** Indicates a provided value is not valid. */ + E_INVALID_VALUE, + + /** Indicates the requested functionality is currently not implemented. */ + E_NOT_IMPLEMENTED, + + /** Indicates the requested functionality is not supported. */ + E_NOT_SUPPORTED, + + /** Indicates the requested item was not found. */ + E_NOT_FOUND, + + /** Indicates the operation timed out. */ + E_TIMEOUT, + + /** Indicates insufficient permission to perform the operation. */ + E_PERMISSION_DENIED, + + /** Indicates an invalid operation was attempted. */ + E_INVALID_OPERATION, + + /** Indicates an event was signaled. */ + E_SIGNALED, + + /** Indicates either not enough memory is available or no memory was allocated */ + E_MEMORY_NOT_ALLOCATED, + + /** VectorNav sensor hard fault (Code 1). */ + E_SENSOR_HARD_FAULT = 1001, + + /** VectorNav sensor serial buffer overflow (Code 2). */ + E_SENSOR_SERIAL_BUFFER_OVERFLOW = 1002, + + /** VectorNav sensor invalid checksum (Code 3). */ + E_SENSOR_INVALID_CHECKSUM = 1003, + + /** VectorNav sensor invalid command (Code 4). */ + E_SENSOR_INVALID_COMMAND = 1004, + + /** VectorNav sensor not enough parameters (Code 5). */ + E_SENSOR_NOT_ENOUGH_PARAMETERS = 1005, + + /** VectorNav sensor too many parameters (Code 6). */ + E_SENSOR_TOO_MANY_PARAMETERS = 1006, + + /** VectorNav sensor invalid parameter (Code 7). */ + E_SENSOR_INVALID_PARAMETER = 1007, + + /** VectorNav sensor invalid register (Code 8). */ + E_SENSOR_INVALID_REGISTER = 1008, + + /** VectorNav sensor unauthorized access (Code 9). */ + E_SENSOR_UNAUTHORIZED_ACCESS = 1009, + + /** VectorNav sensor watchdog reset (Code 10). */ + E_SENSOR_WATCHDOG_RESET = 1010, + + /** VectorNav sensor output buffer overflow (Code 11). */ + E_SENSOR_OUTPUT_BUFFER_OVERFLOW = 1011, + + /** VectorNav sensor insufficient baud rate (Code 12). */ + E_SENSOR_INSUFFICIENT_BAUD_RATE = 1012, + + /** VectorNav sensor error buffer overflow (Code 13). */ + E_SENSOR_ERROR_BUFFER_OVERFLOW = 1013, + + E_DATA_NOT_ELLIPTICAL = 2001, /**< \brief Data set not elliptical. */ + E_ILL_CONDITIONED = 2002, /**< \brief Algorithm had a bad condition. */ + E_EXCEEDED_MAX_ITERATIONS = 2003, /**< \brief Algorithm exceeded the number of interations it is allowed. */ + E_BAD_FINAL_INTERATION = 2004, /**< \brief Algorithm's last interation changed exceeded threshold. */ + E_INSUFFICIENT_DATA = 2005, /**< \brief Not enough data points were provided. */ + + /** \brief Errors that the VectorNav bootload can report. */ + E_BOOTLOADER_NONE = 3000, /**< No Error, send next record*/ + E_BOOTLOADER_INVALID_COMMAND = 3001, /**< Problem with VNX record, abort */ + E_BOOTLOADER_INVALID_RECORD_TYPE = 3002, /**< Problem with VNX record, abort */ + E_BOOTLOADER_INVALID_BYTE_COUNT = 3003, /**< Problem with VNX record, abort */ + E_BOOTLOADER_INVALID_MEMORY_ADDRESS = 3004, /**< Problem with VNX record, abort */ + E_BOOTLOADER_COMM_ERROR = 3005, /**< Checksum error, resend record */ + E_BOOTLOADER_INVALID_HEX_FILE = 3006, /**< Problem with VNX record, abort */ + E_BOOTLOADER_DECRYPTION_ERROR = 3007, /**< Invalid VNX file or record sent out of order, abort */ + E_BOOTLOADER_INVALID_BLOCK_CRC = 3008, /**< Data verification failed, abort */ + E_BOOTLOADER_INVALID_PROGRAM_CRC = 3009, /**< Problemw ith firmware on device */ + E_BOOTLOADER_INVALID_PROGRAM_SIZE = 3010, /**< Problemw ith firmware on device */ + E_BOOTLOADER_MAX_RETRY_COUNT = 3011, /**< Too many errors, abort */ + E_BOOTLOADER_TIMEOUT = 3012, /**< Timeout expired, reset */ + E_BOOTLOADER_RESERVED = 3013 /**< Contact VectorNav, abort */ + +} VnError; + +/** \brief Enumeration of the various error-detection algorithms used by the + * library. */ +typedef enum +{ + /** Signifies no error-detection should be performed. */ + VNERRORDETECTIONMODE_NONE, + + /** Signifies to use 8-bit XOR checksum. */ + VNERRORDETECTIONMODE_CHECKSUM, + + /** Signifies to use CRC16-CCITT algorithm. */ + VNERRORDETECTIONMODE_CRC + +} VnErrorDetectionMode; + +/** \brief Different modes for the SyncInMode field of the Synchronization Control register. */ +typedef enum +{ + #ifdef EXTRA + /** \brief Count the number of trigger events on SYNC_IN_2 pin. + / * \deprecated This option is obsolete for VN-100 firmware version 2.0 and greater and VN-200 firmware version 1.0 and greater. + */ + VNSYNCINMODE_COUNT2 = 0, + /** \brief Start ADC sampling on trigger of SYNC_IN_2 pin. + / * \deprecated This option is obsolete for VN-100 firmware version 2.0 and greater and VN-200 firmware version 1.0 and greater. + */ + VNSYNCINMODE_ADC2 = 1, + /** \brief Output asynchronous message on trigger of SYNC_IN_2 pin. + / * \deprecated This option is obsolete for VN-100 firmware version 2.0 and greater and VN-200 firmware version 1.0 and greater. + */ + VNSYNCINMODE_ASYNC2 = 2, + #endif + /** \brief Count number of trigger events on SYNC_IN pin. + */ + VNSYNCINMODE_COUNT = 3, + /** \brief Start IMU sampling on trigger of SYNC_IN pin. + */ + VNSYNCINMODE_IMU = 4, + /** \brief Output asynchronous message on trigger of SYNC_IN pin. + */ + VNSYNCINMODE_ASYNC = 5 +} VnSyncInMode; + +/** \brief Different modes for the SyncInEdge field of the Synchronization Control register. */ +typedef enum +{ + /** \brief Trigger on the rising edge on the SYNC_IN pin. + */ + VNSYNCINEDGE_RISING = 0, + /** \brief Trigger on the falling edge on the SYNC_IN pin. + */ + VNSYNCINEDGE_FALLING = 1 +} VnSyncInEdge; + +/** \brief Different modes for the SyncOutMode field of the Synchronization Control register. */ +typedef enum +{ + /** \brief None. + */ + VNSYNCOUTMODE_NONE = 0, + /** \brief Trigger at start of IMU sampling. + */ + VNSYNCOUTMODE_ITEMSTART = 1, + /** \brief Trigger when IMU measurements are available. + */ + VNSYNCOUTMODE_IMUREADY = 2, + /** \brief Trigger when attitude measurements are available. + */ + VNSYNCOUTMODE_INS = 3, + /** \brief Trigger on GPS PPS event when a 3D fix is valid. + */ + VNSYNCOUTMODE_GPSPPS = 6 +} VnSyncOutMode; + +/** \brief Different modes for the SyncOutPolarity field of the Synchronization Control register. */ +typedef enum +{ + /** \brief Negative pulse. + */ + VNSYNCOUTPOLARITY_NEGATIVE = 0, + /** \brief Positive pulse. + */ + VNSYNCOUTPOLARITY_POSITIVE = 1 +} VnSyncOutPolarity; + +/** \brief Counting modes for the Communication Protocol Control register. */ +typedef enum +{ + /** \brief Off. + */ + VNCOUNTMODE_NONE = 0, + /** \brief SyncIn counter. + */ + VNCOUNTMODE_SYNCINCOUNT = 1, + /** \brief SyncIn time. + */ + VNCOUNTMODE_SYNCINTIME = 2, + /** \brief SyncOut counter. + */ + VNCOUNTMODE_SYNCOUTCOUNTER = 3, + /** \brief GPS PPS time. + */ + VNCOUNTMODE_GPSPPS = 4 +} VnCountMode; + +/** \brief Status modes for the Communication Protocol Control register. */ +typedef enum +{ + /** \brief Off. + */ + VNSTATUSMODE_OFF = 0, + /** \brief VPE status. + */ + VNSTATUSMODE_VPESTATUS = 1, + /** \brief INS status. + */ + VNSTATUSMODE_INSSTATUS = 2 +} VnStatusMode; + +/** \brief Checksum modes for the Communication Protocol Control register. */ +typedef enum +{ + /** \brief Off. + */ + VNCHECKSUMMODE_OFF = 0, + /** \brief 8-bit checksum. + */ + VNCHECKSUMMODE_CHECKSUM = 1, + /** \brief 16-bit CRC. + */ + VNCHECKSUMMODE_CRC = 2 +} VnChecksumMode; + +/** \brief Error modes for the Communication Protocol Control register. */ +typedef enum +{ + /** \brief Ignore error. + */ + VNERRORMODE_IGNORE = 0, + /** \brief Send error. + */ + VNERRORMODE_SEND = 1, + /** \brief Send error and set ADOR register to off. + */ + VNERRORMODE_SENDANDOFF = 2 +} VnErrorMode; + +/** \brief Filter modes for the IMU Filtering Configuration register. */ +typedef enum +{ + /** \brief No filtering. + */ + VNFILTERMODE_NOFILTERING = 0, + /** \brief Filtering performed only on raw uncompensated IMU measurements. + */ + VNFILTERMODE_ONLYRAW = 1, + /** \brief Filtering performed only on compensated IMU measurements. + */ + VNFILTERMODE_ONLYCOMPENSATED = 2, + /** \brief Filtering performed on both uncompensated and compensated IMU measurements. + */ + VNFILTERMODE_BOTH = 3 +} VnFilterMode; + +/** \brief Integration frames for the Delta Theta and Delta Velocity Configuration register. */ +typedef enum +{ + /** \brief Body frame. + */ + VNINTEGRATIONFRAME_BODY = 0, + /** \brief NED frame. + */ + VNINTEGRATIONFRAME_NED = 1 +} VnIntegrationFrame; + +/** \brief Compensation modes for the Delta Theta and Delta Velocity configuration register. */ +typedef enum +{ + /** \brief None. + */ + VNCOMPENSATIONMODE_NONE = 0, + /** \brief Bias. + */ + VNCOMPENSATIONMODE_BIAS = 1 +} VnCompensationMode; + +/** \brief GPS fix modes for the GPS Solution - LLA register. */ +typedef enum +{ + /** \brief No fix. + */ + VNGPSFIX_NOFIX = 0, + /** \brief Time only. + */ + VNGPSFIX_TIMEONLY = 1, + /** \brief 2D. + */ + VNGPSFIX_2D = 2, + /** \brief 3D. + */ + VNGPSFIX_3D = 3 +} VnGpsFix; + +/** \brief GPS modes for the GPS Configuration register. */ +typedef enum +{ + /** \brief Use onboard GPS. + */ + VNGPSMODE_ONBOARDGPS = 0, + /** \brief Use external GPS. + */ + VNGPSMODE_EXTERNALGPS = 1, + /** \brief Use external VN-200 as GPS. + */ + VNGPSMODE_EXTERNALVN200GPS = 2 +} VnGpsMode; + +/** \brief GPS PPS mode for the GPS Configuration register. */ +typedef enum +{ + /** \brief GPS PPS signal on GPS_PPS pin and triggered on rising edge. + */ + VNPPSSOURCE_GPSPPSRISING = 0, + /** \brief GPS PPS signal on GPS_PPS pin and triggered on falling edge. + */ + VNPPSSOURCE_GPSPPSFALLING = 1, + /** \brief GPS PPS signal on SyncIn pin and triggered on rising edge. + */ + VNPPSSOURCE_SYNCINRISING = 2, + /** \brief GPS PPS signal on SyncIn pin and triggered on falling edge. + */ + VNPPSSOURCE_SYNCINFALLING = 3 +} VnPpsSource; + +/** \brief VPE Enable mode for the VPE Basic Control register. */ +typedef enum +{ + /** \brief Disable + */ + VNVPEENABLE_DISABLE = 0, + /** \brief Enable + */ + VNVPEENABLE_ENABLE = 1 +} VnVpeEnable; + +/** \brief VPE Heading modes used by the VPE Basic Control register. */ +typedef enum +{ + /** \brief Absolute heading. + */ + VNHEADINGMODE_ABSOLUTE = 0, + /** \brief Relative heading. + */ + VNHEADINGMODE_RELATIVE = 1, + /** \brief Indoor heading. + */ + VNHEADINGMODE_INDOOR = 2 +} VnHeadingMode; + +/** \brief VPE modes for the VPE Basic Control register. */ +typedef enum +{ + /** \brief Off. + */ + VNVPEMODE_OFF = 0, + /** \brief Mode 1. + */ + VNVPEMODE_MODE1 = 1 +} VnVpeMode; + +/** \brief Different scenario modes for the INS Basic Configuration register. */ +typedef enum +{ + /** \brief AHRS. + */ + VNSCENARIO_AHRS = 0, + /** \brief General purpose INS with barometric pressure sensor. + */ + VNSCENARIO_INSWITHPRESSURE = 1, + /** \brief General purpose INS without barometric pressure sensor. + */ + VNSCENARIO_INSWITHOUTPRESSURE = 2, + /** \brief GPS moving baseline for dynamic applications. + */ + VNSCENARIO_GPSMOVINGBASELINEDYNAMIC = 3, + /** \brief GPS moving baseline for static applications. + */ + VNSCENARIO_GPSMOVINGBASELINESTATIC = 4 +} VnScenario; + +/** \brief HSI modes used for the Magnetometer Calibration Control register. */ +typedef enum +{ + /** \brief Real-time hard/soft iron calibration algorithm is turned off. + */ + VNHSIMODE_OFF = 0, + /** \brief Runs the real-time hard/soft iron calibration algorithm. + */ + VNHSIMODE_RUN = 1, + /** \brief Resets the real-time hard/soft iron solution. + */ + VNHSIMODE_RESET = 2 +} VnHsiMode; + +/** \brief HSI output types for the Magnetometer Calibration Control register. */ +typedef enum +{ + /** \brief Onboard HSI is not applied to the magnetic measurements. + */ + VNHSIOUTPUT_NOONBOARD = 1, + /** \brief Onboard HSI is applied to the magnetic measurements. + */ + VNHSIOUTPUT_USEONBOARD = 3 +} VnHsiOutput; + +/** \brief Type of velocity compensation performed by the VPE. */ +typedef enum +{ + /** \brief Disabled + */ + VNVELOCITYCOMPENSATIONMODE_DISABLED = 0, + /** \brief Body Measurement + */ + VNVELOCITYCOMPENSATIONMODE_BODYMEASUREMENT = 1 +} VnVelocityCompensationMode; + +/** \brief How the magnetometer is used by the filter. */ +typedef enum +{ + /** \brief Magnetometer will only affect heading. + */ + VNMAGNETICMODE_2D = 0, + /** \brief Magnetometer will affect heading, pitch, and roll. + */ + VNMAGNETICMODE_3D = 1 +} VnMagneticMode; + +/** \brief Source for magnetometer used by the filter. */ +typedef enum +{ + /** \brief Use internal magnetometer. + */ + VNEXTERNALSENSORMODE_INTERNAL = 0, + /** \brief Use external magnetometer. Will use measurement at every new time step. + */ + VNEXTERNALSENSORMODE_EXTERNAL200HZ = 1, + /** \brief Use external magnetometer. Will only use when the measurement is updated. + */ + VNEXTERNALSENSORMODE_EXTERNALONUPDATE = 2 +} VnExternalSensorMode; + +/** \brief Options for the use of FOAM. */ +typedef enum +{ + /** \brief FOAM is not used. + */ + VNFOAMINIT_NOFOAMINIT = 0, + /** \brief FOAM is used to initialize only pitch and roll. + */ + VNFOAMINIT_FOAMINITPITCHROLL = 1, + /** \brief FOAM is used to initialize heading, pitch and roll. + */ + VNFOAMINIT_FOAMINITHEADINGPITCHROLL = 2, + /** \brief FOAM is used to initialize pitch, roll and covariance. + */ + VNFOAMINIT_FOAMINITPITCHROLLCOVARIANCE = 3, + /** \brief FOAM is used to initialize heading, pitch, roll and covariance + */ + VNFOAMINIT_FOAMINITHEADINGPITCHROLLCOVARIANCE = 4 +} VnFoamInit; + +/** \brief The different types of processors. */ +typedef enum +{ + VNPROCESSOR_NAV, /**< Navigation Processor. */ + VNPROCESSOR_GPS, /**< GPS Processor. */ + VNPROCESSOR_IMU /**< IMU Processor. */ +} VnProcessorType; + +#endif \ No newline at end of file diff --git a/vn-interface/include/vn-interface/helper.h b/vn-interface/include/vn-interface/helper.h new file mode 100644 index 0000000..c215573 --- /dev/null +++ b/vn-interface/include/vn-interface/helper.h @@ -0,0 +1,140 @@ +#ifndef _VN200_ +#define _VN200_ + +#include +#include +#include + +#define VN_SPI_READ 0x01 +#define VN_SPI_WRITE 0x02 + +#define VN_ID_IMU_MEASUREMENT_REGISTER 54 + +namespace vn { + +const uint8_t ImuMeasurementsRegisterID = 54; + +/* TODO */ +union header_t { + struct request_t { + uint8_t Cmd = 0; /* Defined in VN200_SPI_READ - set in standard constructor */ + uint8_t ID = 0; /* Register ID */ + uint16_t Empty = 0; /* Spacer */ + request_t(uint8_t ID, uint8_t Cmd) { + this->ID = ID; + this->Cmd = Cmd; + }; + }; + struct response_t { + uint8_t Empty1 = 0; /* Spacer */ + uint8_t Cmd = 0; /* Defined in VN200_SPI_READ - set in standard constructor */ + uint8_t ID = 0; /* Register ID */ + uint8_t Empty2 = 0; /* Spacer */ + response_t(){}; + response_t(uint8_t ID, uint8_t Cmd) { + this->ID = ID; + this->Cmd = Cmd; + }; + }; +}; + +/* Writes the desired payload into the specified register */ +template struct pkg_request_write_t { + struct header_t::request_t header; + payload_t payload; + /* TODO */ + pkg_request_write_t(uint8_t ID) : header(ID, VN_SPI_WRITE){}; +}; + +/* Writes the desired payload into the specified register */ +struct pkg_request_read_t { + struct header_t::request_t header; + /* TODO */ + pkg_request_read_t(uint8_t ID) : header(ID, VN_SPI_READ){}; +}; + +/* Writes the desired payload into the specified register */ +template struct pkg_response_t { + struct header_t::response_t header; + payload_t payload; + /* TODO */ + pkg_response_t() : header(){}; +}; + +// namespace IMU { +// const uint8_t ID = 54; +// struct imu_t { +// float MagX = 0.0; +// float MagY = 0.0; +// float MagZ = 0.0; +// float AccelX = 0.0; +// float AccelY = 0.0; +// float AccelZ = 0.0; +// float GyroX = 0.0; +// float GyroY = 0.0; +// float GyroZ = 0.0; +// float Pressure = 0.0; +// }; +// } // namespace IMU +// namespace ATT { +// const uint8_t ID = 239; +// struct attitude_t { +// float Yaw = 0.0; +// float Pitch = 0.0; +// float Roll = 0.0; +// float BodyAccelX = 0.0; +// float BodyAccelY = 0.0; +// float BodyAccelZ = 0.0; +// float GyroX = 0.0; +// float GyroY = 0.0; +// float GyroZ = 0.0; +// }; +// } // namespace ATT +// namespace INS { +// namespace LLA { +// const uint8_t ID = 63; +// struct lla_t { +// float Yaw = 0.0; +// float Pitch = 0.0; +// float Roll = 0.0; +// double Lattitude = 0.0; +// double Longitude = 0.0; +// double Altitude = 0.0; +// float VelocityX = 0.0; +// float VelocityY = 0.0; +// float VelocityZ = 0.0; +// float AccelX = 0.0; +// float AccelY = 0.0; +// float AccelZ = 0.0; +// float AngularRateX = 0.0; +// float AngularRateY = 0.0; +// float AngularRateZ = 0.0; +// }; +// } // namespace LLA +// } // namespace INS +// namespace GPS { +// namespace LLA { +// const uint8_t ID = 58; +// struct lla_t { +// double Time = 0.0; +// uint16_t Week = 0; +// uint8_t GpsFix = 0; +// uint8_t NumSats = 0; +// uint32_t PADDING = 0; +// double Latitude = 0.0; +// double Longitude = 0.0; +// double Altitude = 0.0; +// float NedVelX = 0.0; +// float NedVelY = 0.0; +// float NedVelZ = 0.0; +// float NorthAcc = 0.0; +// float EastAcc = 0.0; +// float VertAcc = 0.0; +// float SpeedAcc = 0.0; +// float TimeAcc = 0.0; +// }; +// } // namespace LLA +// } // namespace GPS +} // namespace vn + +#endif diff --git a/vn-interface/include/vn-interface/int.h b/vn-interface/include/vn-interface/int.h new file mode 100644 index 0000000..c58b350 --- /dev/null +++ b/vn-interface/include/vn-interface/int.h @@ -0,0 +1,29 @@ +#ifndef VNINT_H_INCLUDED +#define VNINT_H_INCLUDED + +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable : 4820) +#pragma warning(disable : 4255) +#endif + +/* Visual Studio 2008 and earlier do not include the stdint.h header file. */ +#if defined(_MSC_VER) && _MSC_VER <= 1500 + typedef signed __int8 int8_t; + typedef signed __int16 int16_t; + typedef signed __int32 int32_t; + typedef signed __int64 int64_t; + typedef unsigned __int8 uint8_t; + typedef unsigned __int16 uint16_t; + typedef unsigned __int32 uint32_t; + typedef unsigned __int64 uint64_t; +#else + /* Just include the standard header file for integer types. */ + #include +#endif + +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#endif diff --git a/vn-interface/include/vn-interface/math/matrix.h b/vn-interface/include/vn-interface/math/matrix.h new file mode 100644 index 0000000..e187748 --- /dev/null +++ b/vn-interface/include/vn-interface/math/matrix.h @@ -0,0 +1,69 @@ +#ifndef VN_MATRIX_H_INCLUDED +#define VN_MATRIX_H_INCLUDED + +#ifdef __cplusplus +extern "C" { +#endif + +/** \brief Represents a 3x3 matrix with an underlying data type of float. */ +typedef union { + float e[3 * 3]; /**< The matrix's elements in column-major ordering. */ + + struct { + float e00; /**< Element [0,0]. */ + float e10; /**< Element [1,0]. */ + float e20; /**< Element [2,0]. */ + float e01; /**< Element [0,1]. */ + float e11; /**< Element [1,1]. */ + float e21; /**< Element [2,1]. */ + float e02; /**< Element [0,2]. */ + float e12; /**< Element [1,2]. */ + float e22; /**< Element [2,2]. */ + }; + +} mat3f; + +/** \brief Represents a quaternion reading with underlying data type of float. */ +typedef union { + float c[4]; /**< Indexable. */ + + struct { + float x; /**< The x component. */ + float y; /**< The y component. */ + float z; /**< The z component. */ + float w; /**< The w component. */ + }; + + struct { + float c0; /**< Component 0. */ + float c1; /**< Component 1. */ + float c2; /**< Component 2. */ + float c3; /**< Component 2. */ + }; + +} quatf; + +/** \brief Initializes a 3x3 float matrix from an float array with matrix + * elements in column-major ordering. + * + * \param[out] m 3x3 float matrix to initialize + * \param[in] fa float array containing a 3x3 matrix in column-major order */ +void vn_m3_init_fa(mat3f *m, const float *fa); + +/** \brief Converts a mat3f to a string. + * + * \param[out] out The char buffer to output the result to. + * \param[in] m The mat3f to convert. + */ +void strFromMat3f(char *out, mat3f m); + +/** \brief Negates a mat3f. + * \param[in] m Matrix to negate. + * \return Negated matrix. */ +mat3f vnm_negative_mat3f(mat3f m); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/vn-interface/include/vn-interface/math/vector.h b/vn-interface/include/vn-interface/math/vector.h new file mode 100644 index 0000000..d22f25b --- /dev/null +++ b/vn-interface/include/vn-interface/math/vector.h @@ -0,0 +1,146 @@ +#ifndef VN_VECTOR_H_INCLUDED +#define VN_VECTOR_H_INCLUDED + +/** \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. */ + + struct { + float x; /**< X component. */ + float y; /**< Y component. */ + float z; /**< Z component. */ + }; + + struct { + float c0; /**< Component 0. */ + float c1; /**< Component 1. */ + float c2; /**< Component 2. */ + }; + +} vec3f; + +/** \brief Represents a 3 component vector with an underlying data type of + * double. */ +typedef union { + double c[3]; /**< Indexable. */ + + struct { + double x; /**< The x component. */ + double y; /**< The y component. */ + double z; /**< The z component. */ + }; + + struct { + double c0; /**< Component 0. */ + double c1; /**< Component 1. */ + double c2; /**< Component 2. */ + }; + +} vec3d; + +/** \brief Represents a 4 component vector with an underlying data type of + * float. */ +typedef union { + float c[4]; /**< Indexable. */ + + struct { + float x; /**< The x component. */ + float y; /**< The y component. */ + float z; /**< The z component. */ + float w; /**< The w component. */ + }; + + struct { + float c0; /**< Component 0. */ + float c1; /**< Component 1. */ + float c2; /**< Component 2. */ + float c3; /**< Component 2. */ + }; + +} 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/vn-interface/include/vn-interface/registers.h b/vn-interface/include/vn-interface/registers.h new file mode 100644 index 0000000..bebd403 --- /dev/null +++ b/vn-interface/include/vn-interface/registers.h @@ -0,0 +1,846 @@ +/** \file + * {COMMON_HEADER} + * + * \section Description + * This header file contains declarations for using VectorNav sensors. + */ +#ifndef _VNSENSORS_H_ +#define _VNSENSORS_H_ + +#include +#include + +#include "enum.h" +#include "int.h" +#include "math/matrix.h" +#include "math/vector.h" + +#ifdef __cplusplus +extern "C" { +namespace vn { +#endif + +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable : 4820) +#endif + +// Define the max record size of a firmware update file +#define MAXFIRMWAREUPDATERECORDSIZE 300 + +/** \defgroup registerStructures Register Structures + * \brief These structures represent the various registers on a VecotorNav + * sensor. + * + * \{ */ + +/** \brief Structure representing a Binary Output register. + * + * The field outputGroup available on the sensor's register is not necessary + * in this structure since all read/writes operations will automatically + * determine this from the settings for the individual groups within this + * structure. */ +typedef struct { + AsyncMode asyncMode; /**< The asyncMode field. */ + uint16_t rateDivisor; /**< The rateDivisor field. */ + CommonGroup commonField; /**< Group 1 (Common) */ + TimeGroup timeField; /**< Group 2 (Time) */ + ImuGroup imuField; /**< Group 3 (IMU) */ + GpsGroup gpsField; /**< Group 4 (GPS) */ + AttitudeGroup attitudeField; /**< Group 5 (Attitude) */ + InsGroup insField; /**< Group 6 (INS) */ + GpsGroup gps2Field; /**< Group 7 (GPS2) */ +} BinaryOutputRegister; + +/** \brief Initializes a BinaryOutputRegister structure. + * + * \param[in] reg The BinaryOutputRegister structure to initialize. + * \param[in] asyncMode Value to initialize the asyncMode field with. + * \param[in] rateDivisor Value to initialize the rateDivisor field with. + * \param[in] commonField Value to initialize the commonField with. + * \param[in] timeField Value to initialize the timeField with. + * \param[in] imuField Value to initialize the imuField with. + * \param[in] gpsField Value to initialize the gpsField with. + * \param[in] attitudeField Value to initialize the attitudeField with. + * \param[in] insField Value to initialize the insField with. */ +void BinaryOutputRegister_initialize(BinaryOutputRegister *reg, AsyncMode asyncMode, uint32_t rateDivisor, CommonGroup commonField, + TimeGroup timeField, ImuGroup imuField, GpsGroup gpsField, AttitudeGroup attitudeField, InsGroup insField, + GpsGroup gps2Field); + +/** \brief Structure representing the Quaternion, Magnetic, Acceleration and Angular Rates register. */ +typedef struct { + /** \brief The Quat field. */ + vec4f quat; + + /** \brief The Mag field. */ + vec3f mag; + + /** \brief The Accel field. */ + vec3f accel; + + /** \brief The Gyro field. */ + vec3f gyro; + +} QuaternionMagneticAccelerationAndAngularRatesRegister; + +/** \brief Structure representing the Magnetic, Acceleration and Angular Rates register. */ +typedef struct { + /** \brief The Mag field. */ + vec3f mag; + + /** \brief The Accel field. */ + vec3f accel; + + /** \brief The Gyro field. */ + vec3f gyro; + +} MagneticAccelerationAndAngularRatesRegister; + +/** \brief Structure representing the Magnetic and Gravity Reference Vectors register. */ +typedef struct { + /** \brief The MagRef field. */ + vec3f magRef; + + /** \brief The AccRef field. */ + vec3f accRef; + +} MagneticAndGravityReferenceVectorsRegister; + +/** \brief Structure representing the Filter Measurements Variance Parameters register. */ +typedef struct { + /** \brief The Angular Walk Variance field. */ + float angularWalkVariance; + + /** \brief The Angular Rate Variance field. */ + vec3f angularRateVariance; + + /** \brief The Magnetic Variance field. */ + vec3f magneticVariance; + + /** \brief The Acceleration Variance field. */ + vec3f accelerationVariance; + +} FilterMeasurementsVarianceParametersRegister; + +/** \brief Structure representing the Magnetometer Compensation register. */ +typedef struct { + /** \brief The C field. */ + mat3f c; + + /** \brief The B field. */ + vec3f b; + +} MagnetometerCompensationRegister; + +/** \brief Structure representing the Filter Active Tuning Parameters register. */ +typedef struct { + /** \brief The Magnetic Disturbance Gain field. */ + float magneticDisturbanceGain; + + /** \brief The Acceleration Disturbance Gain field. */ + float accelerationDisturbanceGain; + + /** \brief The Magnetic Disturbance Memory field. */ + float magneticDisturbanceMemory; + + /** \brief The Acceleration Disturbance Memory field. */ + float accelerationDisturbanceMemory; + +} FilterActiveTuningParametersRegister; + +/** \brief Structure representing the Acceleration Compensation register. */ +typedef struct { + /** \brief The C field. */ + mat3f c; + + /** \brief The B field. */ + vec3f b; + +} AccelerationCompensationRegister; + +/** \brief Structure representing the Yaw, Pitch, Roll, Magnetic, Acceleration and Angular Rates register. */ +typedef struct { + /** \brief The YawPitchRoll field. */ + vec3f yawPitchRoll; + + /** \brief The Mag field. */ + vec3f mag; + + /** \brief The Accel field. */ + vec3f accel; + + /** \brief The Gyro field. */ + vec3f gyro; + +} YawPitchRollMagneticAccelerationAndAngularRatesRegister; + +/** \brief Structure representing the Communication Protocol Control register. */ +typedef struct { + /** \brief The SerialCount field. */ + uint8_t serialCount; + + /** \brief The SerialStatus field. */ + uint8_t serialStatus; + + /** \brief The SPICount field. */ + uint8_t spiCount; + + /** \brief The SPIStatus field. */ + uint8_t spiStatus; + + /** \brief The SerialChecksum field. */ + uint8_t serialChecksum; + + /** \brief The SPIChecksum field. */ + uint8_t spiChecksum; + + /** \brief The ErrorMode field. */ + uint8_t errorMode; + +} CommunicationProtocolControlRegister; + +/** \brief Structure representing the Synchronization Control register. */ +typedef struct { + /** \brief The SyncInMode field. */ + uint8_t syncInMode; + + /** \brief The SyncInEdge field. */ + uint8_t syncInEdge; + + /** \brief The SyncInSkipFactor field. */ + uint16_t syncInSkipFactor; + + /** \brief The SyncOutMode field. */ + uint8_t syncOutMode; + + /** \brief The SyncOutPolarity field. */ + uint8_t syncOutPolarity; + + /** \brief The SyncOutSkipFactor field. */ + uint16_t syncOutSkipFactor; + + /** \brief The SyncOutPulseWidth field. */ + uint32_t syncOutPulseWidth; + +} SynchronizationControlRegister; + +/** \brief Structure representing the Synchronization Status register. */ +typedef struct { + /** \brief The SyncInCount field. */ + uint32_t syncInCount; + + /** \brief The SyncInTime field. */ + uint32_t syncInTime; + + /** \brief The SyncOutCount field. */ + uint32_t syncOutCount; + +} SynchronizationStatusRegister; + +/** \brief Structure representing the Filter Basic Control register. */ +typedef struct { + /** \brief The MagMode field. */ + uint8_t magMode; + + /** \brief The ExtMagMode field. */ + uint8_t extMagMode; + + /** \brief The ExtAccMode field. */ + uint8_t extAccMode; + + /** \brief The ExtGyroMode field. */ + uint8_t extGyroMode; + + /** \brief The GyroLimit field. */ + vec3f gyroLimit; + +} FilterBasicControlRegister; + +/** \brief Structure representing the VPE Basic Control register. */ +typedef struct { + /** \brief The Enable field. */ + uint8_t enable; + + /** \brief The HeadingMode field. */ + uint8_t headingMode; + + /** \brief The FilteringMode field. */ + uint8_t filteringMode; + + /** \brief The TuningMode field. */ + uint8_t tuningMode; + +} VpeBasicControlRegister; + +/** \brief Structure representing the VPE Magnetometer Basic Tuning register. */ +typedef struct { + /** \brief The BaseTuning field. */ + vec3f baseTuning; + + /** \brief The AdaptiveTuning field. */ + vec3f adaptiveTuning; + + /** \brief The AdaptiveFiltering field. */ + vec3f adaptiveFiltering; + +} VpeMagnetometerBasicTuningRegister; + +/** \brief Structure representing the VPE Magnetometer Advanced Tuning register. */ +typedef struct { + /** \brief The MinFiltering field. */ + vec3f minFiltering; + + /** \brief The MaxFiltering field. */ + vec3f maxFiltering; + + /** \brief The MaxAdaptRate field. */ + float maxAdaptRate; + + /** \brief The DisturbanceWindow field. */ + float disturbanceWindow; + + /** \brief The MaxTuning field. */ + float maxTuning; + +} VpeMagnetometerAdvancedTuningRegister; + +/** \brief Structure representing the VPE Accelerometer Basic Tuning register. */ +typedef struct { + /** \brief The BaseTuning field. */ + vec3f baseTuning; + + /** \brief The AdaptiveTuning field. */ + vec3f adaptiveTuning; + + /** \brief The AdaptiveFiltering field. */ + vec3f adaptiveFiltering; + +} VpeAccelerometerBasicTuningRegister; + +/** \brief Structure representing the VPE Accelerometer Advanced Tuning register. */ +typedef struct { + /** \brief The MinFiltering field. */ + vec3f minFiltering; + + /** \brief The MaxFiltering field. */ + vec3f maxFiltering; + + /** \brief The MaxAdaptRate field. */ + float maxAdaptRate; + + /** \brief The DisturbanceWindow field. */ + float disturbanceWindow; + + /** \brief The MaxTuning field. */ + float maxTuning; + +} VpeAccelerometerAdvancedTuningRegister; + +/** \brief Structure representing the VPE Gyro Basic Tuning register. */ +typedef struct { + /** \brief The AngularWalkVariance field. */ + vec3f angularWalkVariance; + + /** \brief The BaseTuning field. */ + vec3f baseTuning; + + /** \brief The AdaptiveTuning field. */ + vec3f adaptiveTuning; + +} VpeGyroBasicTuningRegister; + +/** \brief Structure representing the Magnetometer Calibration Control register. */ +typedef struct { + /** \brief The HSIMode field. */ + uint8_t hsiMode; + + /** \brief The HSIOutput field. */ + uint8_t hsiOutput; + + /** \brief The ConvergeRate field. */ + uint8_t convergeRate; + +} MagnetometerCalibrationControlRegister; + +/** \brief Structure representing the Calculated Magnetometer Calibration register. */ +typedef struct { + /** \brief The C field. */ + mat3f c; + + /** \brief The B field. */ + vec3f b; + +} CalculatedMagnetometerCalibrationRegister; + +/** \brief Structure representing the Velocity Compensation Control register. */ +typedef struct { + /** \brief The Mode field. */ + uint8_t mode; + + /** \brief The VelocityTuning field. */ + float velocityTuning; + + /** \brief The RateTuning field. */ + float rateTuning; + +} VelocityCompensationControlRegister; + +/** \brief Structure representing the Velocity Compensation Status register. */ +typedef struct { + /** \brief The x field. */ + float x; + + /** \brief The xDot field. */ + float xDot; + + /** \brief The accelOffset field. */ + vec3f accelOffset; + + /** \brief The omega field. */ + vec3f omega; + +} VelocityCompensationStatusRegister; + +/** \brief Structure representing the IMU Measurements register. */ +typedef struct { + /** \brief The Mag field. */ + vec3f mag; + + /** \brief The Accel field. */ + vec3f accel; + + /** \brief The Gyro field. */ + vec3f gyro; + + /** \brief The Temp field. */ + float temp; + + /** \brief The Pressure field. */ + float pressure; + +} ImuMeasurementsRegister; + +/** \brief Structure representing the GPS Configuration register. */ +typedef struct { + /** \brief The Mode field. */ + uint8_t mode; + + /** \brief The PpsSource field. */ + uint8_t ppsSource; + +} GpsConfigurationRegister; + +/** \brief Structure representing the GPS Solution - LLA register. */ +typedef struct { + /** \brief The Time field. */ + double time; + + /** \brief The Week field. */ + uint16_t week; + + /** \brief The GpsFix field. */ + uint8_t gpsFix; + + /** \brief The NumSats field. */ + uint8_t numSats; + + /** \brief The Lla field. */ + vec3d lla; + + /** \brief The NedVel field. */ + vec3f nedVel; + + /** \brief The NedAcc field. */ + vec3f nedAcc; + + /** \brief The SpeedAcc field. */ + float speedAcc; + + /** \brief The TimeAcc field. */ + float timeAcc; + +} GpsSolutionLlaRegister; + +/** \brief Structure representing the GPS Solution - ECEF register. */ +typedef struct { + /** \brief The Tow field. */ + double tow; + + /** \brief The Week field. */ + uint16_t week; + + /** \brief The GpsFix field. */ + uint8_t gpsFix; + + /** \brief The NumSats field. */ + uint8_t numSats; + + /** \brief The Position field. */ + vec3d position; + + /** \brief The Velocity field. */ + vec3f velocity; + + /** \brief The PosAcc field. */ + vec3f posAcc; + + /** \brief The SpeedAcc field. */ + float speedAcc; + + /** \brief The TimeAcc field. */ + float timeAcc; + +} GpsSolutionEcefRegister; + +/** \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; + +#ifdef __cplusplus +} +} +#endif + +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#endif diff --git a/vn-interface/include/vn-interface/types.h b/vn-interface/include/vn-interface/types.h new file mode 100644 index 0000000..27aed1e --- /dev/null +++ b/vn-interface/include/vn-interface/types.h @@ -0,0 +1,56 @@ +#ifndef VN_TYPES_H_INCLUDED +#define VN_TYPES_H_INCLUDED + +/** \brief Standard types used through out the library. */ +#include "int.h" + +#if !defined(__cplusplus) + +#if (defined(__STDC_VERSION__) && __STDC_VERSION__ >= 199901L) || defined(__GNUC__) + +#include + +#else + +/* Must not have C99. */ + +/** Backup definition of size_t. */ +typedef unsigned int size_t; + +#endif + +#else +extern "C" { +#endif + +/* Adding VectorNav data types */ +typedef struct { + int8_t year; /** \brief Year field. */ + uint8_t month; /** \brief Month field. */ + uint8_t day; /** \brief Day field. */ + uint8_t hour; /** \brief Hour field. */ + uint8_t min; /** \brief Min field. */ + uint8_t sec; /** \brief Sec field. */ + uint16_t ms; /** \brief Ms field. */ +} TimeUtc; + +typedef struct { + float gDOP; /** \brief Gdop field. */ + float pDOP; /** \brief Pdop field. */ + float tDOP; /** \brief Tdop field. */ + float vDOP; /** \brief Vdop field. */ + float hDOP; /** \brief Hdop field. */ + float nDOP; /** \brief Ndop field. */ + float eDOP; /** \brief Edop field. */ +} GpsDop; + +typedef struct { + uint8_t timeStatus; /** \brief timeStatus field. */ + int8_t leapSeconds; /** \brief leapSeconds field. */ +} TimeInfo; + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/vn-interface/include/vn-interface/vn200.h b/vn-interface/include/vn-interface/vn200.h deleted file mode 100644 index 39e3c84..0000000 --- a/vn-interface/include/vn-interface/vn200.h +++ /dev/null @@ -1,138 +0,0 @@ -#ifndef _VN200_ -#define _VN200_ - -#include -#include -#include - -#define VN200_SPI_READ 0x01 -#define VN200_SPI_WRITE 0x02 - -namespace VN200 { -namespace SPI { - -/* TODO */ -union header_t { - struct request_t { - uint8_t Cmd = 0; /* Defined in VN200_SPI_READ - set in standard constructor */ - uint8_t ID = 0; /* Register ID */ - uint16_t Empty = 0; /* Spacer */ - request_t(uint8_t ID, uint8_t Cmd) { - this->ID = ID; - this->Cmd = Cmd; - }; - }; - struct response_t { - uint8_t Empty1 = 0; /* Spacer */ - uint8_t Cmd = 0; /* Defined in VN200_SPI_READ - set in standard constructor */ - uint8_t ID = 0; /* Register ID */ - uint8_t Empty2 = 0; /* Spacer */ - response_t(){}; - response_t(uint8_t ID, uint8_t Cmd) { - this->ID = ID; - this->Cmd = Cmd; - }; - }; -}; - -/* Writes the desired payload into the specified register */ -template struct pkg_request_write_t { - struct header_t::request_t header; - payload_t payload; - /* TODO */ - pkg_request_write_t(uint8_t ID) : header(ID, VN200_SPI_WRITE){}; -}; - -/* Writes the desired payload into the specified register */ -struct pkg_request_read_t { - struct header_t::request_t header; - /* TODO */ - pkg_request_read_t(uint8_t ID) : header(ID, VN200_SPI_READ){}; -}; - -/* Writes the desired payload into the specified register */ -template struct pkg_response_t { - struct header_t::response_t header; - payload_t payload; - /* TODO */ - pkg_response_t() : header(){}; -}; - -namespace IMU { -const uint8_t ID = 54; -struct imu_t { - float MagX = 0.0; - float MagY = 0.0; - float MagZ = 0.0; - float AccelX = 0.0; - float AccelY = 0.0; - float AccelZ = 0.0; - float GyroX = 0.0; - float GyroY = 0.0; - float GyroZ = 0.0; - float Pressure = 0.0; -}; -} // namespace IMU -namespace ATT { -const uint8_t ID = 239; -struct attitude_t { - float Yaw = 0.0; - float Pitch = 0.0; - float Roll = 0.0; - float BodyAccelX = 0.0; - float BodyAccelY = 0.0; - float BodyAccelZ = 0.0; - float GyroX = 0.0; - float GyroY = 0.0; - float GyroZ = 0.0; -}; -} // namespace ATT -namespace INS { -namespace LLA { -const uint8_t ID = 63; -struct lla_t { - float Yaw = 0.0; - float Pitch = 0.0; - float Roll = 0.0; - double Lattitude = 0.0; - double Longitude = 0.0; - double Altitude = 0.0; - float VelocityX = 0.0; - float VelocityY = 0.0; - float VelocityZ = 0.0; - float AccelX = 0.0; - float AccelY = 0.0; - float AccelZ = 0.0; - float AngularRateX = 0.0; - float AngularRateY = 0.0; - float AngularRateZ = 0.0; -}; -} // namespace LLA -} // namespace INS -namespace GPS { -namespace LLA { -const uint8_t ID = 58; -struct lla_t { - double Time = 0.0; - uint16_t Week = 0; - uint8_t GpsFix = 0; - uint8_t NumSats = 0; - uint32_t PADDING = 0; - double Latitude = 0.0; - double Longitude = 0.0; - double Altitude = 0.0; - float NedVelX = 0.0; - float NedVelY = 0.0; - float NedVelZ = 0.0; - float NorthAcc = 0.0; - float EastAcc = 0.0; - float VertAcc = 0.0; - float SpeedAcc = 0.0; - float TimeAcc = 0.0; -}; -} // namespace LLA -} // namespace GPS -} // namespace SPI -} // namespace VN200 - -#endif diff --git a/vn-interface/src/helper.cpp b/vn-interface/src/helper.cpp new file mode 100644 index 0000000..185ed12 --- /dev/null +++ b/vn-interface/src/helper.cpp @@ -0,0 +1,3 @@ +#include "vn-interface/vn200.h" + +namespace vn {} // namespace vn diff --git a/vn-interface/src/math/matrix.c b/vn-interface/src/math/matrix.c new file mode 100644 index 0000000..9de581e --- /dev/null +++ b/vn-interface/src/math/matrix.c @@ -0,0 +1,20 @@ +#include "vn-interface/math/matrix.h" + +#include "vn-interface/types.h" + +void vn_m3_init_fa(mat3f *m, const float *fa) { + size_t i; + + for (i = 0; i < 9; i++) + m->e[i] = fa[i]; +} + +mat3f vnm_negative_mat3f(mat3f m) { + mat3f r; + size_t i; + + for (i = 0; i < 3 * 3; i++) + r.e[i] = -m.e[i]; + + return r; +} diff --git a/vn-interface/src/math/vector.c b/vn-interface/src/math/vector.c new file mode 100644 index 0000000..c03bea8 --- /dev/null +++ b/vn-interface/src/math/vector.c @@ -0,0 +1,118 @@ +#include "vn/math/vector.h" + +#include + +vec3d create_v3d(double x, double y, double z) +{ + vec3d v; + + v.c[0] = x; + v.c[1] = y; + v.c[2] = z; + + return v; +} + +void vn_v3_init_fa(vec3f* v, const float* fa) +{ + size_t i; + + for (i = 0; i < 3; i++) + v->c[i] = fa[i]; +} + +vec3f add_v3f_v3f(vec3f lhs, vec3f rhs) +{ + vec3f r; + + r.c[0] = lhs.c[0] + rhs.c[0]; + r.c[1] = lhs.c[1] + rhs.c[1]; + r.c[2] = lhs.c[2] + rhs.c[2]; + + return r; +} + +vec3d add_v3d_v3d(vec3d lhs, vec3d rhs) +{ + vec3d r; + + r.c[0] = lhs.c[0] + rhs.c[0]; + r.c[1] = lhs.c[1] + rhs.c[1]; + r.c[2] = lhs.c[2] + rhs.c[2]; + + return r; +} + +vec4f add_v4f_v4f(vec4f lhs, vec4f rhs) +{ + vec4f r; + + r.c[0] = lhs.c[0] + rhs.c[0]; + r.c[1] = lhs.c[1] + rhs.c[1]; + r.c[2] = lhs.c[2] + rhs.c[2]; + r.c[3] = lhs.c[3] + rhs.c[3]; + + return r; +} + +vec3f sub_v3f_v3f(vec3f lhs, vec3f rhs) +{ + vec3f r; + + r.c[0] = lhs.c[0] - rhs.c[0]; + r.c[1] = lhs.c[1] - rhs.c[1]; + r.c[2] = lhs.c[2] - rhs.c[2]; + + return r; +} + +vec3d sub_v3d_v3d(vec3d lhs, vec3d rhs) +{ + vec3d r; + + r.c[0] = lhs.c[0] - rhs.c[0]; + r.c[1] = lhs.c[1] - rhs.c[1]; + r.c[2] = lhs.c[2] - rhs.c[2]; + + return r; +} + +vec4f sub_v4f_v4f(vec4f lhs, vec4f rhs) +{ + vec4f r; + + r.c[0] = lhs.c[0] - rhs.c[0]; + r.c[1] = lhs.c[1] - rhs.c[1]; + r.c[2] = lhs.c[2] - rhs.c[2]; + r.c[3] = lhs.c[3] - rhs.c[3]; + + return r; +} + +#if defined(_MSC_VER) + /* Disable warnings regarding using sprintf_s since these + * function signatures do not provide us with information + * about the length of 'out'. */ + #pragma warning(push) + #pragma warning(disable:4996) +#endif + +void str_vec3f(char* out, vec3f v) +{ + sprintf(out, "(%f; %f; %f)", v.c[0], v.c[1], v.c[2]); +} + +void str_vec3d(char* out, vec3d v) +{ + sprintf(out, "(%f; %f; %f)", v.c[0], v.c[1], v.c[2]); +} + +void str_vec4f(char* out, vec4f v) +{ + sprintf(out, "(%f; %f; %f; %f)", v.c[0], v.c[1], v.c[2], v.c[3]); +} + + +#if defined(_MSC_VER) + #pragma warning(pop) +#endif diff --git a/vn-interface/src/registers.c b/vn-interface/src/registers.c new file mode 100644 index 0000000..8385439 --- /dev/null +++ b/vn-interface/src/registers.c @@ -0,0 +1,15 @@ +#include "vn-interface/registers.h" + +void BinaryOutputRegister_initialize(BinaryOutputRegister *reg, AsyncMode asyncMode, uint32_t rateDivisor, CommonGroup commonField, + TimeGroup timeField, ImuGroup imuField, GpsGroup gpsField, AttitudeGroup attitudeField, InsGroup insField, + GpsGroup gps2Field) { + reg->asyncMode = asyncMode; + reg->rateDivisor = (uint16_t)rateDivisor; + reg->commonField = commonField; + reg->timeField = timeField; + reg->imuField = imuField; + reg->gpsField = gpsField; + reg->attitudeField = attitudeField; + reg->insField = insField; + reg->gps2Field = gps2Field; +} diff --git a/vn-interface/src/vn200.cpp b/vn-interface/src/vn200.cpp deleted file mode 100644 index 1325604..0000000 --- a/vn-interface/src/vn200.cpp +++ /dev/null @@ -1,5 +0,0 @@ -#include "vn-interface/vn200.h" - -namespace VN200 { -namespace SPI {} // namespace SPI -} // namespace VN200