├── .gitignore ├── .gitlab-ci.yml ├── CHANGELOG.md ├── CMakeLists.txt ├── CONTRIBUTING.md ├── LICENSE.md ├── README.md ├── docs ├── VN-200 with External GNSS Aiding.pdf ├── VN100.pdf ├── VN200.pdf └── VN300.pdf ├── examples ├── arduino │ └── spi_example │ │ └── spi_example.ino └── cmake │ └── spi.cc ├── img ├── arduino_logo_75.png └── logo-words_75.png ├── keywords.txt ├── library.properties └── src ├── elapsedMillis.h ├── registers.h ├── vector_nav.h ├── vn.h ├── vn100.cpp ├── vn100.h ├── vn200.cpp ├── vn200.h ├── vn300.cpp └── vn300.h /.gitignore: -------------------------------------------------------------------------------- 1 | # ignore the build directory 2 | /build* 3 | 4 | # ignore VS code 5 | .vscode 6 | -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | stages: 2 | - lint 3 | 4 | Lint: 5 | stage: lint 6 | tags: 7 | - bfs 8 | script: 9 | - cpplint --verbose=0 src/registers.h 10 | - cpplint --verbose=0 src/vector_nav.h 11 | - cpplint --verbose=0 src/vn.h 12 | - cpplint --verbose=0 src/vn100.h 13 | - cpplint --verbose=0 src/vn200.h 14 | - cpplint --verbose=0 src/vn300.h 15 | - cpplint --verbose=0 src/vn100.cc 16 | - cpplint --verbose=0 src/vn200.cc 17 | - cpplint --verbose=0 src/vn300.cc 18 | 19 | -------------------------------------------------------------------------------- /CHANGELOG.md: -------------------------------------------------------------------------------- 1 | # Changelog 2 | 3 | ## v4.0.4 4 | - Fixed bug where VN-300 SetCompassBaseline was expecting a matrix instead of a vector 5 | 6 | ## v4.0.3 7 | - Updated core to 3.1.3 8 | 9 | ## v4.0.2 10 | - Updated core to 3.1.2 11 | 12 | ## v4.0.1 13 | - Updated core to support MMOD 14 | 15 | ## v4.0.0 16 | - Removed dependency on Eigen and Units library 17 | 18 | ## v3.1.0 19 | - Added default constructor and a Config method to configure the sensor 20 | 21 | ## v3.0.1 22 | - Fixing issue in library.properties 23 | 24 | ## v3.0.0 25 | - Updated to support CMake and Arduino builds 26 | 27 | ## v2.0.3 28 | - Updated to use mcu-support repo 29 | 30 | ## v2.0.2 31 | - Updated tooling for Teensy 4.1 32 | - Updated Vn100, Vn200, and Vn300 *Begin* methods to check for a valid serial number to verify communication. Also checks the model (i.e. VN-300) to ensure the correct object is being used for the sensor. 33 | 34 | ## v2.0.1 35 | - Updated tooling to support Teensy 4.x 36 | 37 | ## v2.0.0 38 | - Updated namespace to *bfs* 39 | - Updated to support units v3.1.0 40 | 41 | ## v1.0.2 42 | - Updated to support core v2.0.4 and units v2.0.0 43 | - Pulling in sources from GitHub 44 | 45 | ## v1.0.1 46 | - Updated to support Teensy 4.x 47 | 48 | ## v1.0.0 49 | - Initial baseline 50 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | if (DEFINED MCU) 3 | include(FetchContent) 4 | FetchContent_Declare( 5 | mcu_support 6 | GIT_REPOSITORY https://github.com/bolderflight/mcu-support.git 7 | GIT_TAG v1.1.0 8 | ) 9 | FetchContent_MakeAvailable(mcu_support) 10 | # Setting up the toolchain 11 | set(CMAKE_TOOLCHAIN_FILE "${mcu_support_SOURCE_DIR}/cmake/cortex.cmake") 12 | # Project information 13 | project(VectorNav 14 | VERSION 4.0.4 15 | DESCRIPTION "VectorNav driver" 16 | LANGUAGES CXX 17 | ) 18 | # Grab the processor and set up definitions and compile options 19 | include(${mcu_support_SOURCE_DIR}/cmake/config_mcu.cmake) 20 | configMcu(${MCU} ${mcu_support_SOURCE_DIR}) 21 | # Fetch core 22 | FetchContent_Declare( 23 | core 24 | GIT_REPOSITORY https://github.com/bolderflight/core.git 25 | GIT_TAG v3.1.3 26 | ) 27 | FetchContent_MakeAvailable(core) 28 | # Add the library target 29 | add_library(vector_nav 30 | src/vector_nav.h 31 | src/registers.h 32 | src/vn.h 33 | src/vn100.h 34 | src/vn200.h 35 | src/vn300.h 36 | src/vn100.cpp 37 | src/vn200.cpp 38 | src/vn300.cpp 39 | ) 40 | # Link libraries 41 | target_link_libraries(vector_nav 42 | PUBLIC 43 | core 44 | ) 45 | # Setup include directories 46 | target_include_directories(vector_nav PUBLIC 47 | $ 48 | $ 49 | ) 50 | # Example and test if this project is built separately 51 | if (PROJECT_NAME STREQUAL CMAKE_PROJECT_NAME) 52 | # Add the spi example target 53 | add_executable(spi_example examples/cmake/spi.cc) 54 | # Add the includes 55 | target_include_directories(spi_example PUBLIC 56 | $ 57 | $ 58 | ) 59 | # Link libraries to the example target 60 | target_link_libraries(spi_example 61 | PRIVATE 62 | vector_nav 63 | ) 64 | # Add hex and upload targets 65 | include(${mcu_support_SOURCE_DIR}/cmake/flash_mcu.cmake) 66 | FlashMcu(spi_example ${MCU} ${mcu_support_SOURCE_DIR}) 67 | endif() 68 | endif() 69 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Contributing 2 | 3 | Please see our [Contributing Guide](https://github.com/bolderflight/contributing) for tips on how to effectively make contributions to our project. 4 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2022 Bolder Flight Systems Inc 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the “Software”), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | 7 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | 9 | THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 10 | 11 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) 2 | 3 | ![Bolder Flight Systems Logo](img/logo-words_75.png)     ![Arduino Logo](img/arduino_logo_75.png) 4 | 5 | # VectorNav 6 | Driver for VectorNav Inertial Measurement Unit (IMU) and Inertial Navigation System (INS) sensors. This library is compatible with Arduino and with CMake build systems. 7 | * [License](LICENSE.md) 8 | * [Changelog](CHANGELOG.md) 9 | * [Contributing guide](CONTRIBUTING.md) 10 | 11 | # Description 12 | VectorNav produces a line of high accuracy IMU and INS sensors. The MEMS sensors in these units are temperature calibrated and an integrated microcontroller provides real-time Extended Kalman Filtering (EKF). The VN-100 is an IMU and Attitude and Heading Reference System (AHRS) providing IMU data and an estimate of the vehicle's attitude and heading. The VN-200 and VN-300 include an integrated GNSS receiver, providing GNSS data and extending the sensor to an Inertial Navigation System (INS), providing high rate estimates of the vehicle's velocity and position in addition to attitude. VN-200 and VN-300 attitude accuracy is higher than the VN-100, especially in flight dynamics and manuevers, such as prolonged turns. The VN-300 utilizes two GNSS receivers to enhance heading accuracy at low speeds. This library communicates with the VN-100, VN-200, and VN-300 sensors using SPI communication. 13 | 14 | # Installation 15 | 16 | ## Arduino 17 | Simply clone or download and extract the zipped library into your Arduino/libraries folder. The library is added as: 18 | 19 | ```C++ 20 | #include "vector_nav.h" 21 | ``` 22 | 23 | An example is located in *examples/arduino/spi_example/spi_example.ino*. This library is tested with Teensy 3.x, 4.x, and LC devices and is expected to work with other Arduino devices. Note that this library won't work with Arduino Uno and possibly other AVR processors, since Arduino Uno treats a *double* as 4 bytes and we really are expecting an 8 byte *double* to handle latitude and longitude data. If you have a recommended fix, please email me or issue a pull request. 24 | 25 | ## CMake 26 | CMake is used to build this library, which is exported as a library target called *vector_nav*. The header is added as: 27 | 28 | ```C++ 29 | #include "vector_nav.h" 30 | ``` 31 | 32 | The library can be also be compiled stand-alone using the CMake idiom of creating a *build* directory and then, from within that directory issuing: 33 | 34 | ``` 35 | cmake .. -DMCU=MK66FX1M0 36 | make 37 | ``` 38 | 39 | This will build the library and an example executables called *spi_example*. The example executable source files are located at *examples/cmake/spi_example.cc*. Notice that the *cmake* command includes a define specifying the microcontroller the code is being compiled for. This is required to correctly configure the code, CPU frequency, and compile/linker options. The available MCUs are: 40 | * MK20DX128 41 | * MK20DX256 42 | * MK64FX512 43 | * MK66FX1M0 44 | * MKL26Z64 45 | * IMXRT1062_T40 46 | * IMXRT1062_T41 47 | * IMXRT1062_MMOD 48 | 49 | These are known to work with the same packages used in Teensy products. Also switching packages is known to work well, as long as it's only a package change. 50 | 51 | The *spi_example* target creates an executable for communicating with the sensor using SPI communication. This target also has a *_hex* for creating the hex file and an *_upload* for using the [Teensy CLI Uploader](https://www.pjrc.com/teensy/loader_cli.html) to flash the Teensy. Instructions for setting up your build environment can be found in our [build-tools repo](https://github.com/bolderflight/build-tools). 52 | 53 | # Namespace 54 | This library is within the namespace *bfs*. 55 | 56 | # Registers 57 | *registers.h* defines all of the VectorNav configuration and data registers. Each register is defined as a struct, whose name matches the register name within the VectorNav User Manuals. The structs define the register ID, the register size, whether it is read only, and the register payload. The payload contains the register fields that are written to or read from. 58 | 59 | The concept is the *VectorNav* class initializes communication with the sensor and provides methods for writing and reading these register structs to/from the sensor. The *VectorNav* class, including structs defined in *registers.h*, enables using any of the sensor's available functionality. The *Vn100*, *Vn200*, and *Vn300* classes wrap around the *VectorNav* class to provide convenience methods for the most common configuration and data collection functions. Whereas these classes provide limited functionality, they provide a more intuitive interface for the majority of use cases. 60 | 61 | # Classes 62 | * [VectorNav](#vector_nav): enables initializing communication and writing / reading register structs. 63 | * [Vn100](#vn100): provides convenience methods for the VN-100 IMU / AHRS. 64 | * [Vn200](#vn200): provides convenience methods for the VN-200 GNSS-aided INS. 65 | * [Vn300](#vn300): provides convenience methods for the VN-300 GNSS-aided INS. 66 | 67 | # VectorNav 68 | This class enables initializing communication with the VectorNav and writing and reading register structs, defined in *registers.h*, to the device. 69 | 70 | ## Methods 71 | 72 | **VectorNav()** Default constructor, requires calling *Config* method to setup the SPI bus and chip select pin. 73 | 74 | **VectorNav(SPIClass *bus, const uint8_t cs)** Constructs a *VectorNav* object given a pointer to the SPI bus object that it is communicating over and the chip select pin number. 75 | 76 | ```C++ 77 | bfs::VectorNav vn(&SPI, 2); 78 | ``` 79 | 80 | **ErrorCode** Most *VectorNav* methods return an error code indicating success or failure of the operation. Below is a table of the error code values: 81 | 82 | | ENUM | Value | 83 | | :-: | :-: | 84 | | ERROR_SUCCESS | 0 | 85 | | ERROR_HARD_FAULT | 1 | 86 | | ERROR_SERIAL_BUFFER_OVERFLOW | 2 | 87 | | ERROR_INVALID_CHECKSUM | 3 | 88 | | ERROR_INVALID_COMMAND | 4 | 89 | | ERROR_NOT_ENOUGH_PARAMETERS | 5 | 90 | | ERROR_TOO_MANY_PARAMETERS | 6 | 91 | | ERROR_INVALID_PARAMETER | 7 | 92 | | ERROR_INVALID_REGISTER | 8 | 93 | | ERROR_UNAUTHORIZED_ACCESS | 9 | 94 | | ERROR_WATCHDOG_RESET | 10 | 95 | | ERROR_OUTPUT_BUFFER_OVERFLOW | 11 | 96 | | ERROR_INSUFFICIENT_BAUD_RATE | 12 | 97 | | ERROR_NULL_PTR | 13 | 98 | | ERROR_NO_COMM | 14 | 99 | | ERROR_WRONG_MODEL | 15 | 100 | | ERROR_ERROR_BUFFER_OVERFLOW | 255 | 101 | 102 | **void Config(SPIClass *bus, const uint8_t cs)** Sets up the SPI bus and chip select pin. Required if using the default constructor. 103 | 104 | **void Init()** Initializes communication with the sensor. **Note:** this method simply initializes the communication bus and chip select pin. It does not test whether communication with the VectorNav sensor is successful. It is recommend to read a register to test for successful communication. The communication bus is not initialized within this library and must be initialized seperately; this enhances compatibility with other sensors that may on the same bus. 105 | 106 | ```C++ 107 | SPI.begin(); 108 | vn.Init(); 109 | ``` 110 | 111 | **ErrorCode ReadRegister(REG *ptr)** Reads a register given a pointer to the register struct. Returns an *ErrorCode* indicating success or failure of the operation. 112 | 113 | ```C++ 114 | VnSerialNumber sn; 115 | ErrorCode err = vn.ReadRegister(&sn); 116 | if (err == ERROR_SUCCESS) { 117 | Serial.println(sn.payload.serial_num); 118 | } 119 | ``` 120 | 121 | **ErrorCode WriteRegister(const REG &ref)** Writes a register given a reference to the register struct. Returns an *ErrorCode* indicating success or failure of the operation. 122 | 123 | ```C++ 124 | VnGnssAntennaOffset ant; 125 | ant.payload.position_x = 2; 126 | ant.payload.position_y = 0; 127 | ant.payload.position_z = 0; 128 | ErrorCode err = vn.WriteRegister(ant); 129 | ``` 130 | 131 | **ErrorCode WriteSettings()** Writes the current settings to the VectorNav non-volatile memory. Returns an *ErrorCode* indicating success or failure of the operation. 132 | 133 | **void RestoreFactorySettings()** Restores the VectorNav to factory default settings. 134 | 135 | **void Reset()** Resets the VectorNav. 136 | 137 | **ErrorCode KnownMagneticDisturbance(bool present)** Notifies the VectorNav that a magnetic disturbance is present. The sensor will tune out the magnetometer and pause the current hard / soft iron calibration, if enabled. A *true* should be passed if a disturbance is present and a *false* passed if a disturbance is no longer present. 138 | 139 | **ErrorCode KnownAccelerationDisturbance(bool present)** Notifies the VectorNav that an acceleration disturbance is present. The sensor will tune out the accelerometer. A *true* should be passed if a disturbance is present and a *false* passed if a disturbance is no longer present. 140 | 141 | **ErrorCode SetGyroBias()** Commands the VectorNav to copy the current gyro bias estimates into volatile memory. These can then be saved in non-volatile memory using the *WriteSettings* command. 142 | 143 | **ErrorCode Tare()** Commands the VectorNav to zero out its current orientation. **Note:** This command is only available on the VN-100. 144 | 145 | **ErrorCode SetFilterBias()** Commands the VectorNav to copy the current filter bias estimates ino volatile memory. These can then be saved in non-volatile memory using the *WriteSettings* command. **Note:** This command is only available on the VN-200 and VN-300. 146 | 147 | # Vn100 148 | This class wraps around the *VectorNav* class to provide convenience methods for the most common functionality using the VN-100 sensor. 149 | 150 | ## Methods 151 | 152 | **Vn100()** Default constructor, requires calling the *Config* method to setup the SPI bus and chip select pin. 153 | 154 | **Vn100(SPIClass *bus, const uint8_t cs)** Constructs a *Vn100* object given a pointer to the SPI bus object that it is communicating over and the chip select pin number. 155 | 156 | ```C++ 157 | bfs::Vn100 vn(&SPI, 2); 158 | ``` 159 | 160 | **VectorNav::ErrorCode error_code()** Most methods within the *Vn100* class return a boolean indicating success or failure of the operation. The [error code](#error_code) from the last operation is returned by this method, which can be useful in debugging if an operation fails. 161 | 162 | ```C++ 163 | if (!vn.Begin()) { 164 | Serial.println(vn.error_code()); 165 | } 166 | ``` 167 | 168 | **void Config(SPIClass *bus, const uint8_t cs)** Sets up the SPI bus and chip select pin. Required if using the default constructor. 169 | 170 | **bool Begin()** Initializes communication with the VN-100 sensor. Returns true on successfully establishing communication and false on failure. The communication bus is not initialized within this library and must be initialized seperately; this enhances compatibility with other sensors that may on the same bus. 171 | 172 | ```C++ 173 | SPI.begin(); 174 | bool status = vn.Begin(); 175 | ``` 176 | 177 | **bool EnableDrdyInt(const DrdyMode mode, const uint16_t srd)** Enables the data ready interrupt, which is a positive pulse with a width of 500 us. The mode enables setting what triggers the interrupt: 178 | 179 | | Enum | Description | 180 | | :-: | :-: | 181 | | IMU_START | Interrupt triggered at the start of IMU sampling | 182 | | IMU_READY | Interrupt triggered when IMU data is ready | 183 | | AHRS | Interrupt triggered when attitude solution is ready | 184 | 185 | The sample rate divider (SRD) sets how many data ready events are skipped before an interrupt is triggered. 186 | 187 | ```C++ 188 | /* Interrupt on attitude solution, 50 Hz rate */ 189 | bool status = vn.EnableDrdyInt(Vn100::AHRS, 7); 190 | ``` 191 | 192 | **bool DisableDrdyInt()** Disables the data ready interrupt. 193 | 194 | **bool ApplyRotation(const float (&C)[M][N])** Applies a rotation. This is useful if the sensor is mounted in a vehicle such that the sensor axes no longer align with the vehicle axes. Rotates the sensor and filter outputs. Outputs are defined as: 195 | 196 | ``` 197 | Xu = c * Xb 198 | ``` 199 | 200 | Where *Xu* is the output, *c* is the rotation matrix, and *Xb* are the measurements in the sensor reference frame. 201 | 202 | ```C++ 203 | /* 204 | * c = 0 1 0 205 | * 1 0 0 206 | * 0 0 1 207 | */ 208 | float c[3][3]; 209 | c[0][1] = 1.0f; 210 | c[1][0] = 1.0f; 211 | c[2][2] = 1.0f; 212 | bool status = vn.ApplyRotation(c); 213 | ``` 214 | 215 | **Note:** The VectorNav needs to write this rotation to non-volatile memory and perform a reset in order for it to be applied. These operations are performed within this method, so it takes a few seconds to complete. 216 | 217 | **bool GetRotation(float (&C)[M][N])** Retrieves the current rotation matrix from the VN-100. 218 | 219 | ```C++ 220 | float c[3][3]; 221 | bool status = vn.GetRotation(c); 222 | ``` 223 | 224 | **Get / Set Filter** The following methods enable setting and getting digital low pass filters for the VectorNav sensors. Each filter can be set to filter the uncompensated, compensated, or both sets of data using the *FilterMode* enum. 225 | 226 | | Enum | Description | 227 | | :-: | :-: | 228 | | FILTER_NONE | No filtering is applied | 229 | | FILTER_UNCOMP_ONLY | Filtering is applied to the uncompensated data only | 230 | | FILTER_COMP_ONLY | Filtering is applied to the compensated data only | 231 | | FILTER_BOTH | Filtering is applied to both the uncompensated and compensated data | 232 | 233 | The filter is tuned by setting a window length for the first order FIR, boxcar filter. 234 | 235 | **bool SetMagFilter(const FilterMode mode, const uint16_t window)** Sets the magnetometer filter. 236 | 237 | **bool GetMagFilter(FilterMode *mode, uint16_t *window)** Gets the magnetometer filter current settings. 238 | 239 | **bool SetAccelFilter(const FilterMode mode, const uint16_t window)** Sets the accelerometer filter. 240 | 241 | **bool GetAccelFilter(FilterMode *mode, uint16_t *window)** Gets the accelerometer filter current settings. 242 | 243 | **bool SetGyroFilter(const FilterMode mode, const uint16_t window)** Sets the gyro filter. 244 | 245 | **bool GetGyroFilter(FilterMode *mode, uint16_t *window)** Gets the gyro filter current settings. 246 | 247 | **bool SetTemperatureFilter(const FilterMode mode, const uint16_t window)** Sets the temperature filter. 248 | 249 | **bool GetTemperatureFilter(FilterMode *mode, uint16_t *window)** Gets the temperature filter current settings. 250 | 251 | **bool SetPressureFilter(const FilterMode mode, const uint16_t window)** Sets the pressure filter. 252 | 253 | **bool GetPressureFilter(FilterMode *mode, uint16_t *window)** Gets the pressure filter current settings. 254 | 255 | **bool DrdyCallback(const uint8_t int_pin, void (*function)())** Enables setting a callback function to be executed on the data ready interrupt, given the pin number of the microcontroller connected to the data ready interrupt pin. 256 | 257 | **bool VelocityCompensation(const float speed_mps)** Compensates the VN-100 accelerometer for turning flight by passing the sensor the current airspeed in m/s. 258 | 259 | **bool Read()** Retrieves the current data from the VN-100 sensor and, on success, stores the updated values in the *Vn100* object. 260 | 261 | ```C++ 262 | if (vn.Read()) { 263 | Serial.print(vn.yaw_rad()); 264 | Serial.print("\t"); 265 | Serial.print(vn.pitch_rad()); 266 | Serial.print("\t"); 267 | Serial.print(vn.roll_rad()); 268 | Serial.print("\n"); 269 | } 270 | ``` 271 | 272 | **float yaw_rad()** Returns the yaw angle, rad. 273 | 274 | **float pitch_rad()** Returns the pitch angle, rad. 275 | 276 | **float roll_rad()** Returns the roll angle, rad. 277 | 278 | **float accel_x_mps2()** Returns the compensated x accelerometer, m/s/s. 279 | 280 | **float accel_y_mps2()** Returns the compensated y accelerometer, m/s/s. 281 | 282 | **float accel_z_mps2()** Returns the compensated z accelerometer, m/s/s. 283 | 284 | **float gyro_x_radps()** Returns the compensated x gyro, rad/s. 285 | 286 | **float gyro_y_radps()** Returns the compensated y gyro, rad/s. 287 | 288 | **float gyro_z_radps()** Returns the compensated z gyro, rad/s. 289 | 290 | **float mag_x_ut()** Returns the compensated x magnetometer, uT. 291 | 292 | **float mag_y_ut()** Returns the compensated y magnetometer, uT. 293 | 294 | **float mag_z_ut()** Returns the compensated z magnetometer, uT. 295 | 296 | **float uncomp_accel_x_mps2()** Returns the uncompensated x accelerometer, m/s/s. 297 | 298 | **float uncomp_accel_y_mps2()** Returns the uncompensated y accelerometer, m/s/s. 299 | 300 | **float uncomp_accel_z_mps2()** Returns the uncompensated z accelerometer, m/s/s. 301 | 302 | **float uncomp_gyro_x_radps()** Returns the uncompensated x gyro, rad/s. 303 | 304 | **float uncomp_gyro_y_radps()** Returns the uncompensated y gyro, rad/s. 305 | 306 | **float uncomp_gyro_z_radps()** Returns the uncompensated z gyro, rad/s. 307 | 308 | **float uncomp_mag_x_ut()** Returns the uncompensated x magnetometer, uT. 309 | 310 | **float uncomp_mag_y_ut()** Returns the uncompensated y magnetometer, uT. 311 | 312 | **float uncomp_mag_z_ut()** Returns the uncompensated z magnetometer, uT. 313 | 314 | **float die_temp_c()** Returns the sensor die temperature, C. 315 | 316 | **float pres_pa()** Returns the measured static pressure, Pa. 317 | 318 | **bool WriteSettings()** Writes the current settings to the VectorNav non-volatile memory. 319 | 320 | **void RestoreFactorySettings()** Restores the VectorNav to factory default settings. 321 | 322 | **void Reset()** Resets the VectorNav. 323 | 324 | **bool KnownMagneticDisturbance(bool present)** Notifies the VectorNav that a magnetic disturbance is present. The sensor will tune out the magnetometer and pause the current hard / soft iron calibration, if enabled. A *true* should be passed if a disturbance is present and a *false* passed if a disturbance is no longer present. 325 | 326 | **bool KnownAccelerationDisturbance(bool present)** Notifies the VectorNav that an acceleration disturbance is present. The sensor will tune out the accelerometer. A *true* should be passed if a disturbance is present and a *false* passed if a disturbance is no longer present. 327 | 328 | **bool SetGyroBias()** Commands the VectorNav to copy the current gyro bias estimates into volatile memory. These can then be saved in non-volatile memory using the *WriteSettings* command. 329 | 330 | **bool Tare()** Commands the VectorNav to zero out its current orientation. 331 | 332 | # Vn200 333 | This class wraps around the *VectorNav* class to provide convenience methods for the most common functionality using the VN-200 sensor. 334 | 335 | ## Methods 336 | 337 | **Vn200()** Default constructor, requires calling the *Config* method to setup the SPI bus and chip select pin. 338 | 339 | **Vn200(SPIClass *bus, const uint8_t cs)** Constructs a *Vn200* object given a pointer to the SPI bus object that is is communicating over and the chip select pin number. 340 | 341 | ```C++ 342 | bfs::Vn200 vn(&SPI, 2); 343 | ``` 344 | 345 | **VectorNav::ErrorCode error_code()** Most methods within the *Vn200* class return a boolean indicating success or failure of the operation. The [error code](#error_code) from the last operation is returned by this method, which can be useful in debugging if an operation fails. 346 | 347 | ```C++ 348 | if (!vn.Begin()) { 349 | Serial.println(vn.error_code()); 350 | } 351 | ``` 352 | 353 | **void Config(SPIClass *bus, const uint8_t cs)** Sets up the SPI bus and chip select pin. Required if using the default constructor. 354 | 355 | **bool Begin()** Initializes communication with the VN-200 sensor. Returns true on successfully establishing communication and false on failure. The communication bus is not initialized within this library and must be initialized seperately; this enhances compatibility with other sensors that may on the same bus. 356 | 357 | ```C++ 358 | SPI.begin(); 359 | bool status = vn.Begin(); 360 | ``` 361 | 362 | **bool EnableDrdyInt(const DrdyMode mode, const uint16_t srd)** Enables the data ready interrupt, which is a positive pulse with a width of 500 us. The mode enables setting what triggers the interrupt: 363 | 364 | | Enum | Description | 365 | | :-: | :-: | 366 | | IMU_START | Interrupt triggered at the start of IMU sampling | 367 | | IMU_READY | Interrupt triggered when IMU data is ready | 368 | | INS | Interrupt triggered when the INS solution is ready | 369 | | GNSS_PPS | Interrupt triggered by the GNSS Pulse-Per-Second (PPS) | 370 | 371 | The sample rate divider (SRD) sets how many data ready events are skipped before an interrupt is triggered. 372 | 373 | ```C++ 374 | /* Interrupt on INS solution, 50 Hz rate */ 375 | bool status = vn.EnableDrdyInt(Vn200::INS, 7); 376 | ``` 377 | 378 | **bool DisableDrdyInt()** Disables the data ready interrupt. 379 | 380 | **bool EnableExternalGnss(const PpsSource pps)** Enables using an external GNSS receiver, rather than the VN-200 internal receiver. The external receiver must send GNSS to the VN-200 at a rate of 5 Hz using the *SendExternalGnssData* method. Additionally, a Pulse-Per-Second (PPS) output from the external receiver must be connected to the VN-200. This method takes an enum parameter specifying the PPS input pin and polarity. 381 | 382 | | Enum | Description | 383 | | :-: | :-: | 384 | | PPS_RISING | External PPS is connected to the VN-200 PPS pin, triggered on a rising edge | 385 | | PPS_FALLING | External PPS is connected to the VN-200 PPS pin, triggered on a falling edge | 386 | | SYNC_IN_RISING | External PPS is connected to the VN-200 sync in pin, triggered on a rising edge | 387 | | SYNC_IN_FALLING | External PPS is connected to the VN-200 sync in pin, triggered on a falling edge | 388 | 389 | **bool DisableExternalGnss()** Disables the external GNSS receiver. 390 | 391 | **bool ApplyRotation(const float (&C)[M][N])** Applies a rotation. This is useful if the sensor is mounted in a vehicle such that the sensor axes no longer align with the vehicle axes. Rotates the sensor and filter outputs. Outputs are defined as: 392 | 393 | ``` 394 | Xu = c * Xb 395 | ``` 396 | 397 | Where *Xu* is the output, *c* is the rotation matrix, and *Xb* are the measurements in the sensor reference frame. 398 | 399 | ```C++ 400 | /* 401 | * c = 0 1 0 402 | * 1 0 0 403 | * 0 0 1 404 | */ 405 | float c[3][3]; 406 | c[0][1] = 1.0f; 407 | c[1][0] = 1.0f; 408 | c[2][2] = 1.0f; 409 | bool status = vn.ApplyRotation(c); 410 | ``` 411 | 412 | **Note:** The VectorNav needs to write this rotation to non-volatile memory and perform a reset in order for it to be applied. These operations are performed within this method, so it takes a few seconds to complete. 413 | 414 | **bool GetRotation(float (&C)[M][N])** Retrieves the current rotation matrix from the VN-100. 415 | 416 | ```C++ 417 | float c[3][3]; 418 | bool status = vn.GetRotation(c); 419 | ``` 420 | 421 | **bool SetAntennaOffset(const float (&b)[M])** Sets the offset from the VN-200 to the antenna. Units are in meters and given in the VN-200 reference frame. 422 | 423 | ```C++ 424 | /* Antenna 2 meters in front on and above the VN-200 */ 425 | float b[3] = {2.0, 0, -2.0}; 426 | bool status = vn.SetAntennaOffset(b); 427 | ``` 428 | 429 | **bool GetAntennaOffset(float (&b)[M])** Retrieves the current antenna offset from the VN-200. 430 | 431 | ```C++ 432 | float b[3]; 433 | bool status = vn.GetAntennaOffset(b); 434 | ``` 435 | 436 | **Get / Set Filter** The following methods enable setting and getting digital low pass filters for the VectorNav sensors. Each filter can be set to filter the uncompensated, compensated, or both sets of data using the *FilterMode* enum. 437 | 438 | | Enum | Description | 439 | | :-: | :-: | 440 | | FILTER_NONE | No filtering is applied | 441 | | FILTER_UNCOMP_ONLY | Filtering is applied to the uncompensated data only | 442 | | FILTER_COMP_ONLY | Filtering is applied to the compensated data only | 443 | | FILTER_BOTH | Filtering is applied to both the uncompensated and compensated data | 444 | 445 | The filter is tuned by setting a window length for the first order FIR, boxcar filter. 446 | 447 | **bool SetMagFilter(const FilterMode mode, const uint16_t window)** Sets the magnetometer filter. 448 | 449 | **bool GetMagFilter(FilterMode *mode, uint16_t *window)** Gets the magnetometer filter current settings. 450 | 451 | **bool SetAccelFilter(const FilterMode mode, const uint16_t window)** Sets the accelerometer filter. 452 | 453 | **bool GetAccelFilter(FilterMode *mode, uint16_t *window)** Gets the accelerometer filter current settings. 454 | 455 | **bool SetGyroFilter(const FilterMode mode, const uint16_t window)** Sets the gyro filter. 456 | 457 | **bool GetGyroFilter(FilterMode *mode, uint16_t *window)** Gets the gyro filter current settings. 458 | 459 | **bool SetTemperatureFilter(const FilterMode mode, const uint16_t window)** Sets the temperature filter. 460 | 461 | **bool GetTemperatureFilter(FilterMode *mode, uint16_t *window)** Gets the temperature filter current settings. 462 | 463 | **bool SetPressureFilter(const FilterMode mode, const uint16_t window)** Sets the pressure filter. 464 | 465 | **bool GetPressureFilter(FilterMode *mode, uint16_t *window)** Gets the pressure filter current settings. 466 | 467 | **bool DrdyCallback(const uint8_t int_pin, void (*function)())** Enables setting a callback function to be executed on the data ready interrupt, given the pin number of the microcontroller connected to the data ready interrupt pin. 468 | 469 | **bool SendExternalGnssData(const VnGnssSolutionLla &ref)** Sends external GNSS data to the VN-200 given a reference to the LLA solution register. The VN-200 must first be configured to use an external GNSS receiver using the *EnableExternalGnss* method. 470 | 471 | **bool SendExternalGnssData(const VnGnssSolutionEcef &ref)** Sends external GNSS data to the VN-200 given a reference to the ECEF solution register. The VN-200 must first be configured to use an external GNSS receiver using the *EnableExternalGnss* method. 472 | 473 | **bool Read()** Retrieves the current data from the VN-200 sensor and, on success, stores the updated values in the *Vn200* object. 474 | 475 | ```C++ 476 | if (vn.Read()) { 477 | Serial.print(vn.yaw_rad()); 478 | Serial.print("\t"); 479 | Serial.print(vn.pitch_rad()); 480 | Serial.print("\t"); 481 | Serial.print(vn.roll_rad()); 482 | Serial.print("\n"); 483 | } 484 | ``` 485 | 486 | **InsMode ins_mode()** Returns the current INS mode, which is an enum. 487 | 488 | | Enum | Description | 489 | | :-: | :-: | 490 | | NOT_TRACKING | The INS filter is awaiting initialization and not currently tracking. | 491 | | DEGRADED | The INS filter is tracking, but the performance is below specifications. This can occur during initial alignment or if the attitude uncertainty rises above 2 degrees. | 492 | | HEALTHY | The INS filter is tracking within specification. | 493 | | GNSS_LOSS | Communication has been lost from the GNSS for more than 45 seconds. The INS will no longer update position or velocity estimates, but the attitude solution remains valid | 494 | 495 | **bool ins_error()** Returns true if an INS error has occurred. 496 | 497 | **bool ins_time_error()** Returns true if a time error has occurred. 498 | 499 | **bool ins_imu_error()** Returns true if an IMU read error has occurred. 500 | 501 | **bool ins_mag_pres_error()** Returns true if a magnetometer or pressure read error has occurred. 502 | 503 | **bool ins_gnss_error()** Returns true if a GNSS receiver read error has occurred. 504 | 505 | **double ins_time_s()** Returns the INS time of week in seconds. This is equivalent to the GNSS time of week, but updated at the INS rate. 506 | 507 | **uint16_t ins_week()** Returns the INS week number. This is equivalent to the GNSS week number, but updated at the INS rate. 508 | 509 | **double ins_lat_rad()** Returns the latitude from the INS, rad. 510 | 511 | **double ins_lon_rad()** Returns the longitude from the INS, rad. 512 | 513 | **double ins_alt_m()** Returns the altitude above the WGS-84 ellipsoid from the INS, m. 514 | 515 | **float ins_north_vel_mps()** Returns the INS velocity in the north direction, m/s. 516 | 517 | **float ins_east_vel_mps()** Returns the INS velocity in the east direction, m/s. 518 | 519 | **float ins_down_vel_mps()** Returns the INS velocity in the down direction, m/s. 520 | 521 | **float ins_att_uncertainty_rad()** Returns the INS attitude estimation uncertainty, rad. 522 | 523 | **float ins_pos_uncertainty()** Returns the INS position estimation uncertainty, m. 524 | 525 | **float ins_vel_uncertainty()** Returns the INS velocity estimation uncertainty, m/s. 526 | 527 | **double gnss_time_s()** Returns the GNSS time of week, seconds. 528 | 529 | **uint16_t gnss_week()** Returns the GNSS week number. 530 | 531 | **GnssFix gnss_fix()** Returns the GNSS fix status. 532 | 533 | | Enum | Description | 534 | | :-: | :-: | 535 | | FIX_NONE | No fix | 536 | | FIX_TIME_ONLY | Time only fix | 537 | | FIX_2D | 2D fix | 538 | | FIX_3D | 3D fix | 539 | | FIX_SBAS | 3D fix with corrections from SBAS | 540 | 541 | **uint8_t gnss_num_satellites()** Returns the number of satellites used in the GNSS solution. 542 | 543 | **double gnss_lat_rad()** Returns the GNSS latitude, rad. 544 | 545 | **double gnss_lon_rad()** Returns the GNSS longitude, rad. 546 | 547 | **double gnss_alt_m()** Returns the GNSS altitude above the WGS-84 ellipsoid, m. 548 | 549 | **float gnss_north_vel_mps()** Returns the GNSS velocity in the north direction, m/s. 550 | 551 | **float gnss_east_vel_mps()** Returns the GNSS velocity in the east direction, m/s. 552 | 553 | **float gnss_down_vel_mps()** Returns the GNSS velocity in the down direction, m/s. 554 | 555 | **float gnss_north_acc_m()** Returns the estimated GNSS position accuracy in the north direction, m. 556 | 557 | **float gnss_east_acc_m()** Returns the estimated GNSS position accuracy in the east direction, m. 558 | 559 | **float gnss_down_acc_m()** Returns the estimated GNSS position accuracy in the down direction, m. 560 | 561 | **float gnss_speed_acc_mps()** Returns the estimated GNSS speed accuracy, m/s. 562 | 563 | **float gnss_time_acc_s()** Returns the estimated GNSS time accuracy, s. 564 | 565 | **float yaw_rad()** Returns the yaw angle relative to true north, rad. 566 | 567 | **float pitch_rad()** Returns the pitch angle, rad. 568 | 569 | **float roll_rad()** Returns the roll angle, rad. 570 | 571 | **float accel_x_mps2()** Returns the compensated x accelerometer, m/s/s. 572 | 573 | **float accel_y_mps2()** Returns the compensated y accelerometer, m/s/s. 574 | 575 | **float accel_z_mps2()** Returns the compensated z accelerometer, m/s/s. 576 | 577 | **float gyro_x_radps()** Returns the compensated x gyro, rad/s. 578 | 579 | **float gyro_y_radps()** Returns the compensated y gyro, rad/s. 580 | 581 | **float gyro_z_radps()** Returns the compensated z gyro, rad/s. 582 | 583 | **float mag_x_ut()** Returns the compensated x magnetometer, uT. 584 | 585 | **float mag_y_ut()** Returns the compensated y magnetometer, uT. 586 | 587 | **float mag_z_ut()** Returns the compensated z magnetometer, uT. 588 | 589 | **float uncomp_accel_x_mps2()** Returns the uncompensated x accelerometer, m/s/s. 590 | 591 | **float uncomp_accel_y_mps2()** Returns the uncompensated y accelerometer, m/s/s. 592 | 593 | **float uncomp_accel_z_mps2()** Returns the uncompensated z accelerometer, m/s/s. 594 | 595 | **float uncomp_gyro_x_radps()** Returns the uncompensated x gyro, rad/s. 596 | 597 | **float uncomp_gyro_y_radps()** Returns the uncompensated y gyro, rad/s. 598 | 599 | **float uncomp_gyro_z_radps()** Returns the uncompensated z gyro, rad/s. 600 | 601 | **float uncomp_mag_x_ut()** Returns the uncompensated x magnetometer, uT. 602 | 603 | **float uncomp_mag_y_ut()** Returns the uncompensated y magnetometer, uT. 604 | 605 | **float uncomp_mag_z_ut()** Returns the uncompensated z magnetometer, uT. 606 | 607 | **float die_temp_c()** Returns the sensor die temperature, C. 608 | 609 | **float pres_pa()** Returns the measured static pressure, Pa. 610 | 611 | **bool WriteSettings()** Writes the current settings to the VectorNav non-volatile memory. 612 | 613 | **void RestoreFactorySettings()** Restores the VectorNav to factory default settings. 614 | 615 | **void Reset()** Resets the VectorNav. 616 | 617 | **bool KnownMagneticDisturbance(bool present)** Notifies the VectorNav that a magnetic disturbance is present. The sensor will tune out the magnetometer and pause the current hard / soft iron calibration, if enabled. A *true* should be passed if a disturbance is present and a *false* passed if a disturbance is no longer present. 618 | 619 | **bool KnownAccelerationDisturbance(bool present)** Notifies the VectorNav that an acceleration disturbance is present. The sensor will tune out the accelerometer. A *true* should be passed if a disturbance is present and a *false* passed if a disturbance is no longer present. 620 | 621 | **bool SetGyroBias()** Commands the VectorNav to copy the current gyro bias estimates into volatile memory. These can then be saved in non-volatile memory using the *WriteSettings* command. 622 | 623 | **bool SetFilterBias()** Commands the VectorNav to copy the current filter bias estimates ino volatile memory. These can then be saved in non-volatile memory using the *WriteSettings* command. 624 | 625 | # Vn300 626 | This class wraps around the *VectorNav* class to provide convenience methods for the most common functionality using the VN-300 sensor. 627 | 628 | ## Methods 629 | 630 | **Vn300()** Default constructor, requires calling the *Config* method to setup the SPI bus and chip select pin. 631 | 632 | **Vn300(SPIClass *bus, const uint8_t cs)** Constructs a *Vn300* object given a pointer to the SPI bus object that it is communicating over and the chip select pin number. 633 | 634 | ```C++ 635 | bfs::Vn300 vn(&SPI, 2); 636 | ``` 637 | 638 | **VectorNav::ErrorCode error_code()** Most methods within the *Vn300* class return a boolean indicating success or failure of the operation. The [error code](#error_code) from the last operation is returned by this method, which can be useful in debugging if an operation fails. 639 | 640 | ```C++ 641 | if (!vn.Begin()) { 642 | Serial.println(vn.error_code()); 643 | } 644 | ``` 645 | 646 | **void Config(SPIClass *bus, const uint8_t cs)** Sets up the SPI bus and chip select pin. Required if using the default constructor. 647 | 648 | **bool Begin()** Initializes communication with the VN-300 sensor. Returns true on successfully establishing communication and false on failure. The communication bus is not initialized within this library and must be initialized seperately; this enhances compatibility with other sensors that may on the same bus. 649 | 650 | ```C++ 651 | SPI.begin(); 652 | bool status = vn.Begin(); 653 | ``` 654 | 655 | **bool EnableDrdyInt(const DrdyMode mode, const uint16_t srd)** Enables the data ready interrupt, which is a positive pulse with a width of 500 us. The mode enables setting what triggers the interrupt: 656 | 657 | | Enum | Description | 658 | | :-: | :-: | 659 | | IMU_START | Interrupt triggered at the start of IMU sampling | 660 | | IMU_READY | Interrupt triggered when IMU data is ready | 661 | | INS | Interrupt triggered when the INS solution is ready | 662 | | GNSS_PPS | Interrupt triggered by the GNSS Pulse-Per-Second (PPS) | 663 | 664 | The sample rate divider (SRD) sets how many data ready events are skipped before an interrupt is triggered. 665 | 666 | ```C++ 667 | /* Interrupt on INS solution, 50 Hz rate */ 668 | bool status = vn.EnableDrdyInt(Vn300::INS, 7); 669 | ``` 670 | 671 | **bool DisableDrdyInt()** Disables the data ready interrupt. 672 | 673 | **bool ApplyRotation(const float (&C)[M][N])** Applies a rotation. This is useful if the sensor is mounted in a vehicle such that the sensor axes no longer align with the vehicle axes. Rotates the sensor and filter outputs. Outputs are defined as: 674 | 675 | ``` 676 | Xu = c * Xb 677 | ``` 678 | 679 | Where *Xu* is the output, *c* is the rotation matrix, and *Xb* are the measurements in the sensor reference frame. 680 | 681 | ```C++ 682 | /* 683 | * c = 0 1 0 684 | * 1 0 0 685 | * 0 0 1 686 | */ 687 | float c[3][3]; 688 | c[0][1] = 1.0f; 689 | c[1][0] = 1.0f; 690 | c[2][2] = 1.0f; 691 | bool status = vn.ApplyRotation(c); 692 | ``` 693 | 694 | **Note:** The VectorNav needs to write this rotation to non-volatile memory and perform a reset in order for it to be applied. These operations are performed within this method, so it takes a few seconds to complete. 695 | 696 | **bool GetRotation(float (&C)[M][N])** Retrieves the current rotation matrix from the VN-100. 697 | 698 | ```C++ 699 | float c[3][3]; 700 | bool status = vn.GetRotation(c); 701 | ``` 702 | 703 | **bool SetAntennaOffset(const float (&b)[M])** Sets the offset from the VN-200 to the antenna. Units are in meters and given in the VN-200 reference frame. 704 | 705 | ```C++ 706 | /* Antenna 2 meters in front on and above the VN-200 */ 707 | float b[3] = {2.0, 0, -2.0}; 708 | bool status = vn.SetAntennaOffset(b); 709 | ``` 710 | 711 | **bool GetAntennaOffset(float (&b)[M])** Retrieves the current antenna offset from the VN-200. 712 | 713 | ```C++ 714 | float b[3]; 715 | bool status = vn.GetAntennaOffset(b); 716 | ``` 717 | 718 | **bool SetCompassBaseline(const float (&pos)[M][N], const float (&uncert)[M][N])** Sets the position of the second GNSS antenna relative to the first GNSS antenna in the VN-300 reference frame, meters. Accuracy of the GNSS heading is related to the baseline distance. In addition to the baseline position, uncertainty in each direction must also be provided, meters. 719 | 720 | ```C++ 721 | /* Second antenna is 1 meter in front of first antenna with an uncertainty of 1 cm */ 722 | float b[3] = {1.0, 0, 0.0}; 723 | float c[3] = {0.01, 0, 0.0}; 724 | bool status = vn.SetCompassBaseline(b, c); 725 | ``` 726 | 727 | **bool GetCompassBaseline(float (&pos)[M][N], float (&uncert)[M][N])** Retrieves the current compass baseline from the VN-300. 728 | 729 | ```C++ 730 | float b[3], c[3]; 731 | bool status = vn.GetCompassBaseline(b, c); 732 | ``` 733 | 734 | **Get / Set Filter** The following methods enable setting and getting digital low pass filters for the VectorNav sensors. Each filter can be set to filter the uncompensated, compensated, or both sets of data using the *FilterMode* enum. 735 | 736 | | Enum | Description | 737 | | :-: | :-: | 738 | | FILTER_NONE | No filtering is applied | 739 | | FILTER_UNCOMP_ONLY | Filtering is applied to the uncompensated data only | 740 | | FILTER_COMP_ONLY | Filtering is applied to the compensated data only | 741 | | FILTER_BOTH | Filtering is applied to both the uncompensated and compensated data | 742 | 743 | The filter is tuned by setting a window length for the first order FIR, boxcar filter. 744 | 745 | **bool SetMagFilter(const FilterMode mode, const uint16_t window)** Sets the magnetometer filter. 746 | 747 | **bool GetMagFilter(FilterMode *mode, uint16_t *window)** Gets the magnetometer filter current settings. 748 | 749 | **bool SetAccelFilter(const FilterMode mode, const uint16_t window)** Sets the accelerometer filter. 750 | 751 | **bool GetAccelFilter(FilterMode *mode, uint16_t *window)** Gets the accelerometer filter current settings. 752 | 753 | **bool SetGyroFilter(const FilterMode mode, const uint16_t window)** Sets the gyro filter. 754 | 755 | **bool GetGyroFilter(FilterMode *mode, uint16_t *window)** Gets the gyro filter current settings. 756 | 757 | **bool SetTemperatureFilter(const FilterMode mode, const uint16_t window)** Sets the temperature filter. 758 | 759 | **bool GetTemperatureFilter(FilterMode *mode, uint16_t *window)** Gets the temperature filter current settings. 760 | 761 | **bool SetPressureFilter(const FilterMode mode, const uint16_t window)** Sets the pressure filter. 762 | 763 | **bool GetPressureFilter(FilterMode *mode, uint16_t *window)** Gets the pressure filter current settings. 764 | 765 | **bool DrdyCallback(const uint8_t int_pin, void (*function)())** Enables setting a callback function to be executed on the data ready interrupt, given the pin number of the microcontroller connected to the data ready interrupt pin. 766 | 767 | **bool Read()** Retrieves the current data from the VN-300 sensor and, on success, stores the updated values in the *Vn300* object. 768 | 769 | ```C++ 770 | if (vn.Read()) { 771 | Serial.print(vn.yaw_rad()); 772 | Serial.print("\t"); 773 | Serial.print(vn.pitch_rad()); 774 | Serial.print("\t"); 775 | Serial.print(vn.roll_rad()); 776 | Serial.print("\n"); 777 | } 778 | ``` 779 | 780 | **InsMode ins_mode()** Returns the current INS mode, which is an enum. 781 | 782 | | Enum | Description | 783 | | :-: | :-: | 784 | | NOT_TRACKING | The INS filter is awaiting initialization and not currently tracking. | 785 | | DEGRADED | The INS filter is tracking, but the performance is below specifications. This can occur during initial alignment or if the attitude uncertainty rises above 2 degrees. | 786 | | HEALTHY | The INS filter is tracking within specification. | 787 | | GNSS_LOSS | Communication has been lost from the GNSS for more than 45 seconds. The INS will no longer update position or velocity estimates, but the attitude solution remains valid | 788 | 789 | **bool ins_error()** Returns true if an INS error has occurred. 790 | 791 | **bool ins_time_error()** Returns true if a time error has occurred. 792 | 793 | **bool ins_imu_error()** Returns true if an IMU read error has occurred. 794 | 795 | **bool ins_mag_pres_error()** Returns true if a magnetometer or pressure read error has occurred. 796 | 797 | **bool ins_gnss_error()** Returns true if a GNSS receiver read error has occurred. 798 | 799 | **bool ins_gnss_heading()** Returns true if the GNSS compass is currently aiding the INS filter heading solution. 800 | 801 | **bool ins_gnss_compass()** Returns true if the GNSS compass is currently operational and reporting a heading solution. 802 | 803 | **double ins_time_s()** Returns the INS time of week in seconds. This is equivalent to the GNSS time of week, but updated at the INS rate. 804 | 805 | **uint16_t ins_week()** Returns the INS week number. This is equivalent to the GNSS week number, but updated at the INS rate. 806 | 807 | **double ins_lat_rad()** Returns the latitude from the INS, rad. 808 | 809 | **double ins_lon_rad()** Returns the longitude from the INS, rad. 810 | 811 | **double ins_alt_m()** Returns the altitude above the WGS-84 ellipsoid from the INS, m. 812 | 813 | **float ins_north_vel_mps()** Returns the INS velocity in the north direction, m/s. 814 | 815 | **float ins_east_vel_mps()** Returns the INS velocity in the east direction, m/s. 816 | 817 | **float ins_down_vel_mps()** Returns the INS velocity in the down direction, m/s. 818 | 819 | **float ins_att_uncertainty_rad()** Returns the INS attitude estimation uncertainty, rad. 820 | 821 | **float ins_pos_uncertainty()** Returns the INS position estimation uncertainty, m. 822 | 823 | **float ins_vel_uncertainty()** Returns the INS velocity estimation uncertainty, m/s. 824 | 825 | **double gnss_time_s()** Returns the GNSS time of week, seconds. 826 | 827 | **uint16_t gnss_week()** Returns the GNSS week number. 828 | 829 | **GnssFix gnss_fix()** Returns the GNSS fix status. 830 | 831 | | Enum | Description | 832 | | :-: | :-: | 833 | | FIX_NONE | No fix | 834 | | FIX_TIME_ONLY | Time only fix | 835 | | FIX_2D | 2D fix | 836 | | FIX_3D | 3D fix | 837 | | FIX_SBAS | 3D fix with corrections from SBAS | 838 | 839 | **uint8_t gnss_num_satellites()** Returns the number of satellites used in the GNSS solution. 840 | 841 | **double gnss_lat_rad()** Returns the GNSS latitude, rad. 842 | 843 | **double gnss_lon_rad()** Returns the GNSS longitude, rad. 844 | 845 | **double gnss_alt_m()** Returns the GNSS altitude above the WGS-84 ellipsoid, m. 846 | 847 | **float gnss_north_vel_mps()** Returns the GNSS velocity in the north direction, m/s. 848 | 849 | **float gnss_east_vel_mps()** Returns the GNSS velocity in the east direction, m/s. 850 | 851 | **float gnss_down_vel_mps()** Returns the GNSS velocity in the down direction, m/s. 852 | 853 | **float gnss_north_acc_m()** Returns the estimated GNSS position accuracy in the north direction, m. 854 | 855 | **float gnss_east_acc_m()** Returns the estimated GNSS position accuracy in the east direction, m. 856 | 857 | **float gnss_down_acc_m()** Returns the estimated GNSS position accuracy in the down direction, m. 858 | 859 | **float gnss_speed_acc_mps()** Returns the estimated GNSS speed accuracy, m/s. 860 | 861 | **float gnss_time_acc_s()** Returns the estimated GNSS time accuracy, s. 862 | 863 | **float yaw_rad()** Returns the yaw angle relative to true north, rad. 864 | 865 | **float pitch_rad()** Returns the pitch angle, rad. 866 | 867 | **float roll_rad()** Returns the roll angle, rad. 868 | 869 | **float accel_x_mps2()** Returns the compensated x accelerometer, m/s/s. 870 | 871 | **float accel_y_mps2()** Returns the compensated y accelerometer, m/s/s. 872 | 873 | **float accel_z_mps2()** Returns the compensated z accelerometer, m/s/s. 874 | 875 | **float gyro_x_radps()** Returns the compensated x gyro, rad/s. 876 | 877 | **float gyro_y_radps()** Returns the compensated y gyro, rad/s. 878 | 879 | **float gyro_z_radps()** Returns the compensated z gyro, rad/s. 880 | 881 | **float mag_x_ut()** Returns the compensated x magnetometer, uT. 882 | 883 | **float mag_y_ut()** Returns the compensated y magnetometer, uT. 884 | 885 | **float mag_z_ut()** Returns the compensated z magnetometer, uT. 886 | 887 | **float uncomp_accel_x_mps2()** Returns the uncompensated x accelerometer, m/s/s. 888 | 889 | **float uncomp_accel_y_mps2()** Returns the uncompensated y accelerometer, m/s/s. 890 | 891 | **float uncomp_accel_z_mps2()** Returns the uncompensated z accelerometer, m/s/s. 892 | 893 | **float uncomp_gyro_x_radps()** Returns the uncompensated x gyro, rad/s. 894 | 895 | **float uncomp_gyro_y_radps()** Returns the uncompensated y gyro, rad/s. 896 | 897 | **float uncomp_gyro_z_radps()** Returns the uncompensated z gyro, rad/s. 898 | 899 | **float uncomp_mag_x_ut()** Returns the uncompensated x magnetometer, uT. 900 | 901 | **float uncomp_mag_y_ut()** Returns the uncompensated y magnetometer, uT. 902 | 903 | **float uncomp_mag_z_ut()** Returns the uncompensated z magnetometer, uT. 904 | 905 | **float die_temp_c()** Returns the sensor die temperature, C. 906 | 907 | **float pres_pa()** Returns the measured static pressure, Pa. 908 | 909 | **bool WriteSettings()** Writes the current settings to the VectorNav non-volatile memory. 910 | 911 | **void RestoreFactorySettings()** Restores the VectorNav to factory default settings. 912 | 913 | **void Reset()** Resets the VectorNav. 914 | 915 | **bool KnownMagneticDisturbance(bool present)** Notifies the VectorNav that a magnetic disturbance is present. The sensor will tune out the magnetometer and pause the current hard / soft iron calibration, if enabled. A *true* should be passed if a disturbance is present and a *false* passed if a disturbance is no longer present. 916 | 917 | **bool KnownAccelerationDisturbance(bool present)** Notifies the VectorNav that an acceleration disturbance is present. The sensor will tune out the accelerometer. A *true* should be passed if a disturbance is present and a *false* passed if a disturbance is no longer present. 918 | 919 | **bool SetGyroBias()** Commands the VectorNav to copy the current gyro bias estimates into volatile memory. These can then be saved in non-volatile memory using the *WriteSettings* command. 920 | 921 | **bool SetFilterBias()** Commands the VectorNav to copy the current filter bias estimates ino volatile memory. These can then be saved in non-volatile memory using the *WriteSettings* command. 922 | -------------------------------------------------------------------------------- /docs/VN-200 with External GNSS Aiding.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bolderflight/vector-nav/7905385e4755e614e449681bacd3b49304be7fc7/docs/VN-200 with External GNSS Aiding.pdf -------------------------------------------------------------------------------- /docs/VN100.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bolderflight/vector-nav/7905385e4755e614e449681bacd3b49304be7fc7/docs/VN100.pdf -------------------------------------------------------------------------------- /docs/VN200.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bolderflight/vector-nav/7905385e4755e614e449681bacd3b49304be7fc7/docs/VN200.pdf -------------------------------------------------------------------------------- /docs/VN300.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bolderflight/vector-nav/7905385e4755e614e449681bacd3b49304be7fc7/docs/VN300.pdf -------------------------------------------------------------------------------- /examples/arduino/spi_example/spi_example.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * Brian R Taylor 3 | * brian.taylor@bolderflight.com 4 | * 5 | * Copyright (c) 2021 Bolder Flight Systems Inc 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the “Software”), to 9 | * deal in the Software without restriction, including without limitation the 10 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | * sell copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in 15 | * all copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | * IN THE SOFTWARE. 24 | */ 25 | 26 | #include "vector_nav.h" 27 | 28 | bfs::Vn300 vn(&SPI, 34); 29 | 30 | void setup() { 31 | Serial.begin(115200); 32 | while(!Serial) {} 33 | /* Initialize communication */ 34 | SPI.begin(); 35 | if (!vn.Begin()) { 36 | Serial.print("Error Code: "); 37 | Serial.println(vn.error_code()); 38 | while (1) {} 39 | } 40 | } 41 | 42 | void loop() { 43 | /* Read sensor and print values */ 44 | if (vn.Read()) { 45 | Serial.print(vn.yaw_rad() * 180 / 3.14159); 46 | Serial.print("\t"); 47 | Serial.print(vn.pitch_rad() * 180 / 3.14159); 48 | Serial.print("\t"); 49 | Serial.println(vn.roll_rad() * 180 / 3.14159); 50 | } 51 | delay(50); 52 | } 53 | 54 | -------------------------------------------------------------------------------- /examples/cmake/spi.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Brian R Taylor 3 | * brian.taylor@bolderflight.com 4 | * 5 | * Copyright (c) 2021 Bolder Flight Systems Inc 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the “Software”), to 9 | * deal in the Software without restriction, including without limitation the 10 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | * sell copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in 15 | * all copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | * IN THE SOFTWARE. 24 | */ 25 | 26 | #include "vector_nav.h" 27 | 28 | bfs::Vn300 vn; 29 | 30 | int main() { 31 | Serial.begin(115200); 32 | while(!Serial) {} 33 | /* Initialize communication */ 34 | SPI.begin(); 35 | vn.Config(&SPI, 34); 36 | if (!vn.Begin()) { 37 | Serial.print("Error Code: "); 38 | Serial.println(vn.error_code()); 39 | while (1) {} 40 | } 41 | while (1) { 42 | /* Read sensor and print values */ 43 | if (vn.Read()) { 44 | Serial.print(vn.yaw_rad() * 180 / 3.14159); 45 | Serial.print("\t"); 46 | Serial.print(vn.pitch_rad() * 180 / 3.14159); 47 | Serial.print("\t"); 48 | Serial.println(vn.roll_rad() * 180 / 3.14159); 49 | } 50 | delay(50); 51 | } 52 | } 53 | 54 | -------------------------------------------------------------------------------- /img/arduino_logo_75.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bolderflight/vector-nav/7905385e4755e614e449681bacd3b49304be7fc7/img/arduino_logo_75.png -------------------------------------------------------------------------------- /img/logo-words_75.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bolderflight/vector-nav/7905385e4755e614e449681bacd3b49304be7fc7/img/logo-words_75.png -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | VectorNav KEYWORD1 2 | ErrorCode KEYWORD1 3 | ERROR_SUCCESS LITERAL1 4 | ERROR_HARD_FAULT LITERAL1 5 | ERROR_SERIAL_BUFFER_OVERFLOW LITERAL1 6 | ERROR_INVALID_CHECKSUM LITERAL1 7 | ERROR_INVALID_COMMAND LITERAL1 8 | ERROR_NOT_ENOUGH_PARAMETERS LITERAL1 9 | ERROR_TOO_MANY_PARAMETERS LITERAL1 10 | ERROR_INVALID_PARAMETER LITERAL1 11 | ERROR_INVALID_REGISTER LITERAL1 12 | ERROR_UNAUTHORIZED_ACCESS LITERAL1 13 | ERROR_WATCHDOG_RESET LITERAL1 14 | ERROR_OUTPUT_BUFFER_OVERFLOW LITERAL1 15 | ERROR_INSUFFICIENT_BAUD_RATE LITERAL1 16 | ERROR_NULL_PTR LITERAL1 17 | ERROR_NO_COMM LITERAL1 18 | ERROR_WRONG_MODEL LITERAL1 19 | ERROR_ERROR_BUFFER_OVERFLOW LITERAL1 20 | Init KEYWORD2 21 | ReadRegister KEYWORD2 22 | WriteRegister KEYWORD2 23 | WriteSettings KEYWORD2 24 | RestoreFactorySettings KEYWORD2 25 | Tare KEYWORD2 26 | Reset KEYWORD2 27 | KnownMagneticDisturbance KEYWORD2 28 | KnownAccelerationDisturbance KEYWORD2 29 | SetGyroBias KEYWORD2 30 | SetFilterBias KEYWORD2 31 | DrdyMode KEYWORD1 32 | IMU_START LITERAL1 33 | IMU_READY LITERAL1 34 | INS LITERAL1 35 | AHRS LITERAL1 36 | GNSS_PPS LITERAL1 37 | FilterMode KEYWORD1 38 | FILTER_NONE LITERAL1 39 | FILTER_UNCOMP_ONLY LITERAL1 40 | FILTER_COMP_ONLY LITERAL1 41 | FILTER_BOTH LITERAL1 42 | Vn100 KEYWORD1 43 | Config KEYWORD2 44 | Begin KEYWORD2 45 | EnableDrdyInt KEYWORD2 46 | DisableDrdyInt KEYWORD2 47 | ApplyRotation KEYWORD2 48 | GetRotation KEYWORD2 49 | SetMagFilter KEYWORD2 50 | GetMagFilter KEYWORD2 51 | SetAccelFilter KEYWORD2 52 | GetAccelFilter KEYWORD2 53 | SetGyroFilter KEYWORD2 54 | GetGyroFilter KEYWORD2 55 | SetTemperatureFilter KEYWORD2 56 | GetTemperatureFilter KEYWORD2 57 | SetPressureFilter KEYWORD2 58 | GetPressureFilter KEYWORD2 59 | DrdyCallback KEYWORD2 60 | VelocityCompensation KEYWORD2 61 | Read KEYWORD2 62 | error_code KEYWORD2 63 | yaw_rad KEYWORD2 64 | pitch_rad KEYWORD2 65 | roll_rad KEYWORD2 66 | accel_x_mps2 KEYWORD2 67 | accel_y_mps2 KEYWORD2 68 | accel_z_mps2 KEYWORD2 69 | gyro_x_radps KEYWORD2 70 | gyro_y_radps KEYWORD2 71 | gyro_z_radps KEYWORD2 72 | mag_x_ut KEYWORD2 73 | mag_y_ut KEYWORD2 74 | mag_z_ut KEYWORD2 75 | uncomp_accel_x_mps2 KEYWORD2 76 | uncomp_accel_y_mps2 KEYWORD2 77 | uncomp_accel_z_mps2 KEYWORD2 78 | uncomp_gyro_x_radps KEYWORD2 79 | uncomp_gyro_y_radps KEYWORD2 80 | uncomp_gyro_z_radps KEYWORD2 81 | uncomp_mag_x_ut KEYWORD2 82 | uncomp_mag_y_ut KEYWORD2 83 | uncomp_mag_z_ut KEYWORD2 84 | die_temp_c KEYWORD2 85 | pres_pa KEYWORD2 86 | Vn200 KEYWORD1 87 | InsMode KEYWORD1 88 | NOT_TRACKING LITERAL1 89 | DEGRADED LITERAL1 90 | HEALTHY LITERAL1 91 | GNSS_LOSS LITERAL1 92 | GnssFix KEYWORD1 93 | FIX_NONE LITERAL1 94 | FIX_TIME_ONLY LITERAL1 95 | FIX_2D LITERAL1 96 | FIX_3D LITERAL1 97 | FIX_SBAS LITERAL1 98 | PpsSource KEYWORD1 99 | PPS_RISING LITERAL1 100 | PPS_FALLING LITERAL1 101 | SYNC_IN_RISING LITERAL1 102 | SYNC_IN_FALLING LITERAL1 103 | EnableExternalGnss KEYWORD2 104 | DisableExternalGnss KEYWORD2 105 | SetAntennaOffset KEYWORD2 106 | GetAntennaOffset KEYWORD2 107 | SendExternalGnssData KEYWORD2 108 | SendExternalGnssData KEYWORD2 109 | ins_mode KEYWORD2 110 | ins_error KEYWORD2 111 | ins_time_error KEYWORD2 112 | ins_imu_error KEYWORD2 113 | ins_mag_pres_error KEYWORD2 114 | ins_gnss_error KEYWORD2 115 | ins_time_s KEYWORD2 116 | ins_week KEYWORD2 117 | ins_lat_rad KEYWORD2 118 | ins_lon_rad KEYWORD2 119 | ins_alt_m KEYWORD2 120 | ins_north_vel_mps KEYWORD2 121 | ins_east_vel_mps KEYWORD2 122 | ins_down_vel_mps KEYWORD2 123 | ins_att_uncertainty_rad KEYWORD2 124 | ins_pos_uncertainty_m KEYWORD2 125 | ins_vel_uncertainty_mps KEYWORD2 126 | gnss_time_s KEYWORD2 127 | gnss_week KEYWORD2 128 | gnss_fix KEYWORD2 129 | gnss_num_sats KEYWORD2 130 | gnss_lat_rad KEYWORD2 131 | gnss_lon_rad KEYWORD2 132 | gnss_alt_m KEYWORD2 133 | gnss_north_vel_mps KEYWORD2 134 | gnss_east_vel_mps KEYWORD2 135 | gnss_down_vel_mps KEYWORD2 136 | gnss_north_acc_m KEYWORD2 137 | gnss_east_acc_m KEYWORD2 138 | gnss_down_acc_m KEYWORD2 139 | gnss_speed_acc_mps KEYWORD2 140 | gnss_time_acc_s KEYWORD2 141 | Vn300 KEYWORD1 142 | SetCompassBaseline KEYWORD2 143 | GetCompassBaseline KEYWORD2 144 | ins_gnss_compass KEYWORD2 145 | ins_gnss_heading KEYWORD2 -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=Bolder Flight Systems VectorNav 2 | version=4.0.4 3 | author=Brian Taylor 4 | maintainer=Brian Taylor 5 | sentence=Library for communicating with VectorNav IMU and INS sensors. 6 | paragraph=This library reads VectorNav VN-100, VN-200, and VN-300 sensors using SPI. 7 | category=Sensors 8 | url=https://github.com/bolderflight/vector_nav 9 | architectures=* 10 | includes=vector_nav.h 11 | depends=Bolder Flight Systems Units, Bolder Flight Systems Eigen 12 | -------------------------------------------------------------------------------- /src/elapsedMillis.h: -------------------------------------------------------------------------------- 1 | /* Elapsed time types - for easy-to-use measurements of elapsed time 2 | * http://www.pjrc.com/teensy/ 3 | * Copyright (c) 2011 PJRC.COM, LLC 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy 6 | * of this software and associated documentation files (the "Software"), to deal 7 | * in the Software without restriction, including without limitation the rights 8 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | * copies of the Software, and to permit persons to whom the Software is 10 | * furnished to do so, subject to the following conditions: 11 | * 12 | * The above copyright notice and this permission notice shall be included in 13 | * all copies or substantial portions of the Software. 14 | * 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | * THE SOFTWARE. 22 | */ 23 | 24 | #ifndef elapsedMillis_h 25 | #define elapsedMillis_h 26 | #ifdef __cplusplus 27 | 28 | #if ARDUINO >= 100 29 | #include "Arduino.h" 30 | #else 31 | #include "WProgram.h" 32 | #endif 33 | 34 | class elapsedMillis 35 | { 36 | private: 37 | unsigned long ms; 38 | public: 39 | elapsedMillis(void) { ms = millis(); } 40 | elapsedMillis(unsigned long val) { ms = millis() - val; } 41 | elapsedMillis(const elapsedMillis &orig) { ms = orig.ms; } 42 | operator unsigned long () const { return millis() - ms; } 43 | elapsedMillis & operator = (const elapsedMillis &rhs) { ms = rhs.ms; return *this; } 44 | elapsedMillis & operator = (unsigned long val) { ms = millis() - val; return *this; } 45 | elapsedMillis & operator -= (unsigned long val) { ms += val ; return *this; } 46 | elapsedMillis & operator += (unsigned long val) { ms -= val ; return *this; } 47 | elapsedMillis operator - (int val) const { elapsedMillis r(*this); r.ms += val; return r; } 48 | elapsedMillis operator - (unsigned int val) const { elapsedMillis r(*this); r.ms += val; return r; } 49 | elapsedMillis operator - (long val) const { elapsedMillis r(*this); r.ms += val; return r; } 50 | elapsedMillis operator - (unsigned long val) const { elapsedMillis r(*this); r.ms += val; return r; } 51 | elapsedMillis operator + (int val) const { elapsedMillis r(*this); r.ms -= val; return r; } 52 | elapsedMillis operator + (unsigned int val) const { elapsedMillis r(*this); r.ms -= val; return r; } 53 | elapsedMillis operator + (long val) const { elapsedMillis r(*this); r.ms -= val; return r; } 54 | elapsedMillis operator + (unsigned long val) const { elapsedMillis r(*this); r.ms -= val; return r; } 55 | }; 56 | 57 | class elapsedMicros 58 | { 59 | private: 60 | unsigned long us; 61 | public: 62 | elapsedMicros(void) { us = micros(); } 63 | elapsedMicros(unsigned long val) { us = micros() - val; } 64 | elapsedMicros(const elapsedMicros &orig) { us = orig.us; } 65 | operator unsigned long () const { return micros() - us; } 66 | elapsedMicros & operator = (const elapsedMicros &rhs) { us = rhs.us; return *this; } 67 | elapsedMicros & operator = (unsigned long val) { us = micros() - val; return *this; } 68 | elapsedMicros & operator -= (unsigned long val) { us += val ; return *this; } 69 | elapsedMicros & operator += (unsigned long val) { us -= val ; return *this; } 70 | elapsedMicros operator - (int val) const { elapsedMicros r(*this); r.us += val; return r; } 71 | elapsedMicros operator - (unsigned int val) const { elapsedMicros r(*this); r.us += val; return r; } 72 | elapsedMicros operator - (long val) const { elapsedMicros r(*this); r.us += val; return r; } 73 | elapsedMicros operator - (unsigned long val) const { elapsedMicros r(*this); r.us += val; return r; } 74 | elapsedMicros operator + (int val) const { elapsedMicros r(*this); r.us -= val; return r; } 75 | elapsedMicros operator + (unsigned int val) const { elapsedMicros r(*this); r.us -= val; return r; } 76 | elapsedMicros operator + (long val) const { elapsedMicros r(*this); r.us -= val; return r; } 77 | elapsedMicros operator + (unsigned long val) const { elapsedMicros r(*this); r.us -= val; return r; } 78 | }; 79 | 80 | #endif // __cplusplus 81 | #endif // elapsedMillis_h -------------------------------------------------------------------------------- /src/registers.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Brian R Taylor 3 | * brian.taylor@bolderflight.com 4 | * 5 | * Copyright (c) 2022 Bolder Flight Systems Inc 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the “Software”), to 9 | * deal in the Software without restriction, including without limitation the 10 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | * sell copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in 15 | * all copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | * IN THE SOFTWARE. 24 | */ 25 | 26 | #ifndef VECTOR_NAV_SRC_REGISTERS_H_ // NOLINT 27 | #define VECTOR_NAV_SRC_REGISTERS_H_ 28 | 29 | #if defined(ARDUINO) 30 | #include 31 | #pragma pack(4) 32 | #else 33 | #include 34 | #endif 35 | 36 | namespace bfs { 37 | 38 | /* 39 | * Defines all the available VectorNav registers with their id and data. 40 | * Namespaces are used to separate common registers from unit 41 | * specific registers (i.e. registers only available on VN-300). 42 | */ 43 | 44 | /* Registers common accross VectorNav products */ 45 | 46 | struct VnUserTag { 47 | static const uint8_t id = 0; 48 | static const uint8_t size = 20; 49 | static const bool read_only = false; 50 | struct { 51 | char tag[20]; 52 | } payload; 53 | }; 54 | 55 | struct VnModelNumber { 56 | static const uint8_t id = 1; 57 | static const uint8_t size = 24; 58 | static const bool read_only = true; 59 | struct { 60 | char product_name[24]; 61 | } payload; 62 | }; 63 | 64 | struct VnHardwareRevision { 65 | static const uint8_t id = 2; 66 | static const uint8_t size = 4; 67 | static const bool read_only = true; 68 | struct { 69 | uint32_t revision; 70 | } payload; 71 | }; 72 | 73 | struct VnSerialNumber { 74 | static const uint8_t id = 3; 75 | static const uint8_t size = 4; 76 | static const bool read_only = true; 77 | struct { 78 | uint32_t serial_num; 79 | } payload; 80 | }; 81 | 82 | struct VnFirmwareVersion { 83 | static const uint8_t id = 4; 84 | static const uint8_t size = 4; 85 | static const bool read_only = true; 86 | struct { 87 | uint8_t major_version; 88 | uint8_t minor_version; 89 | uint8_t feature_version; 90 | uint8_t hotfix; 91 | } payload; 92 | }; 93 | 94 | struct VnSynchronizationControl { 95 | static const uint8_t id = 32; 96 | static const uint8_t size = 20; 97 | static const bool read_only = false; 98 | struct { 99 | uint8_t sync_in_mode; 100 | uint8_t sync_in_edge; 101 | uint16_t sync_in_skip_factor; 102 | uint32_t resv1 = 0; 103 | uint8_t sync_out_mode; 104 | uint8_t sync_out_polarity; 105 | uint16_t sync_out_skip_factor; 106 | uint32_t sync_out_pulse_width; 107 | uint32_t resv2 = 0; 108 | } payload; 109 | }; 110 | 111 | struct VnSynchronizationStatus { 112 | static const uint8_t id = 33; 113 | static const uint8_t size = 12; 114 | static const bool read_only = false; 115 | struct { 116 | uint32_t sync_in_count; 117 | uint32_t sync_in_time; 118 | uint32_t sync_out_count; 119 | } payload; 120 | }; 121 | 122 | struct VnImuMeasurements { 123 | static const uint8_t id = 54; 124 | static const uint8_t size = 44; 125 | static const bool read_only = true; 126 | struct { 127 | float mag_x; 128 | float mag_y; 129 | float mag_z; 130 | float accel_x; 131 | float accel_y; 132 | float accel_z; 133 | float gyro_x; 134 | float gyro_y; 135 | float gyro_z; 136 | float temp; 137 | float pressure; 138 | } payload; 139 | }; 140 | 141 | struct VnDeltaThetaDeltaVelocity { 142 | static const uint8_t id = 80; 143 | static const uint8_t size = 28; 144 | static const bool read_only = true; 145 | struct { 146 | float delta_time; 147 | float delta_theta_x; 148 | float delta_theta_y; 149 | float delta_theta_z; 150 | float delta_velocity_x; 151 | float delta_velocity_y; 152 | float delta_velocity_z; 153 | } payload; 154 | }; 155 | 156 | struct VnMagnetometerCompensation { 157 | static const uint8_t id = 23; 158 | static const uint8_t size = 48; 159 | static const bool read_only = false; 160 | struct { 161 | float c[3][3]; 162 | float b[3]; 163 | } payload; 164 | }; 165 | 166 | struct VnAccelerationCompensation { 167 | static const uint8_t id = 25; 168 | static const uint8_t size = 48; 169 | static const bool read_only = false; 170 | struct { 171 | float c[3][3]; 172 | float b[3]; 173 | } payload; 174 | }; 175 | 176 | struct VnGyroCompensation { 177 | static const uint8_t id = 84; 178 | static const uint8_t size = 48; 179 | static const bool read_only = false; 180 | struct { 181 | float c[3][3]; 182 | float b[3]; 183 | } payload; 184 | }; 185 | 186 | struct VnReferenceFrameRotation { 187 | static const uint8_t id = 26; 188 | static const uint8_t size = 36; 189 | static const bool read_only = false; 190 | struct { 191 | float c[3][3]; 192 | } payload; 193 | }; 194 | 195 | struct VnImuFilteringConfiguration { 196 | static const uint8_t id = 85; 197 | static const uint8_t size = 16; // tested to work with 16 198 | static const bool read_only = false; 199 | struct { 200 | uint16_t mag_window_size; 201 | uint16_t accel_window_size; 202 | uint16_t gyro_window_size; 203 | uint16_t temp_window_size; 204 | uint16_t pres_window_size; 205 | uint8_t mag_filter_mode; 206 | uint8_t accel_filter_mode; 207 | uint8_t gyro_filter_mode; 208 | uint8_t temp_filter_mode; 209 | uint8_t pres_filter_mode; 210 | uint8_t padding; 211 | } payload; 212 | }; 213 | 214 | struct VnDeltaThetaDeltaVelocityConfiguration { 215 | static const uint8_t id = 82; 216 | static const uint8_t size = 6; 217 | static const bool read_only = false; 218 | struct { 219 | uint8_t integration_frame; 220 | uint8_t gyro_compensation; 221 | uint8_t accel_compensation; 222 | uint8_t resv1 = 0; 223 | uint16_t resv2 = 0; 224 | } payload; 225 | }; 226 | 227 | struct VnYawPitchRoll { 228 | static const uint8_t id = 8; 229 | static const uint8_t size = 12; 230 | static const bool read_only = true; 231 | struct { 232 | float yaw; 233 | float pitch; 234 | float roll; 235 | } payload; 236 | }; 237 | 238 | struct VnAttitudeQuaternion { 239 | static const uint8_t id = 9; 240 | static const uint8_t size = 16; 241 | static const bool read_only = true; 242 | struct { 243 | float quat[4]; 244 | } payload; 245 | }; 246 | 247 | struct VnYawPitchRollMagneticAccelerationAngularRates { 248 | static const uint8_t id = 27; 249 | static const uint8_t size = 48; 250 | static const bool read_only = true; 251 | struct { 252 | float yaw; 253 | float pitch; 254 | float roll; 255 | float mag_x; 256 | float mag_y; 257 | float mag_z; 258 | float accel_x; 259 | float accel_y; 260 | float accel_z; 261 | float gyro_x; 262 | float gyro_y; 263 | float gyro_z; 264 | } payload; 265 | }; 266 | 267 | struct VnQuaternionMagneticAccelerationAngularRates { 268 | static const uint8_t id = 15; 269 | static const uint8_t size = 52; 270 | static const bool read_only = true; 271 | struct { 272 | float quat[4]; 273 | float mag_x; 274 | float mag_y; 275 | float mag_z; 276 | float accel_x; 277 | float accel_y; 278 | float accel_z; 279 | float gyro_x; 280 | float gyro_y; 281 | float gyro_z; 282 | } payload; 283 | }; 284 | 285 | struct VnMagneticMeasurements { 286 | static const uint8_t id = 17; 287 | static const uint8_t size = 12; 288 | static const bool read_only = true; 289 | struct { 290 | float mag_x; 291 | float mag_y; 292 | float mag_z; 293 | } payload; 294 | }; 295 | 296 | struct VnAccelerationMeasurements { 297 | static const uint8_t id = 18; 298 | static const uint8_t size = 12; 299 | static const bool read_only = true; 300 | struct { 301 | float accel_x; 302 | float accel_y; 303 | float accel_z; 304 | } payload; 305 | }; 306 | 307 | struct VnAngularRateMeasurements { 308 | static const uint8_t id = 19; 309 | static const uint8_t size = 12; 310 | static const bool read_only = true; 311 | struct { 312 | float gyro_x; 313 | float gyro_y; 314 | float gyro_z; 315 | } payload; 316 | }; 317 | 318 | struct VnMagneticAccelerationAngularRates { 319 | static const uint8_t id = 20; 320 | static const uint8_t size = 36; 321 | static const bool read_only = true; 322 | struct { 323 | float mag_x; 324 | float mag_y; 325 | float mag_z; 326 | float accel_x; 327 | float accel_y; 328 | float accel_z; 329 | float gyro_x; 330 | float gyro_y; 331 | float gyro_z; 332 | } payload; 333 | }; 334 | 335 | struct VnYawPitchRollTrueBodyAccelerationAngularRates { 336 | static const uint8_t id = 239; 337 | static const uint8_t size = 36; 338 | static const bool read_only = true; 339 | struct { 340 | float yaw; 341 | float pitch; 342 | float roll; 343 | float body_accel_x; 344 | float body_accel_y; 345 | float body_accel_z; 346 | float gyro_x; 347 | float gyro_y; 348 | float gyro_z; 349 | } payload; 350 | }; 351 | 352 | struct VnYawPitchRollTrueInertialAccelerationAngularRates { 353 | static const uint8_t id = 240; 354 | static const uint8_t size = 36; 355 | static const bool read_only = true; 356 | struct { 357 | float yaw; 358 | float pitch; 359 | float roll; 360 | float inertial_accel_x; 361 | float inertial_accel_y; 362 | float inertial_accel_z; 363 | float gyro_x; 364 | float gyro_y; 365 | float gyro_z; 366 | } payload; 367 | }; 368 | 369 | struct VnHeave { 370 | static const uint8_t id = 115; 371 | static const uint8_t size = 12; 372 | static const bool read_only = true; 373 | struct { 374 | float heave; 375 | float heave_rate; 376 | float delayed_heave; 377 | } payload; 378 | }; 379 | 380 | struct VnHeaveConfiguration { 381 | static const uint8_t id = 116; 382 | static const uint8_t size = 28; 383 | static const bool read_only = false; 384 | struct { 385 | float initial_wave_period; 386 | float initial_wave_amplitude; 387 | float max_wave_period; 388 | float min_wave_amplitude; 389 | float delayed_heave_cutoff_freq; 390 | float heave_cutoff_freq; 391 | float heave_rate_cutoff_freq; 392 | } payload; 393 | }; 394 | 395 | struct VnVpeBasicControl { 396 | static const uint8_t id = 35; 397 | static const uint8_t size = 4; 398 | static const bool read_only = false; 399 | struct { 400 | uint8_t enable; 401 | uint8_t heading_mode; 402 | uint8_t filtering_mode; 403 | uint8_t tuning_mode; 404 | } payload; 405 | }; 406 | 407 | struct VnMagnetometerCalibrationControl { 408 | static const uint8_t id = 44; 409 | static const uint8_t size = 4; 410 | static const bool read_only = false; 411 | struct { 412 | uint8_t hsi_mode; 413 | uint8_t hsi_output; 414 | uint8_t converge_rate; 415 | } payload; 416 | }; 417 | 418 | struct VnCalculatedMagnetometerCalibration { 419 | static const uint8_t id = 47; 420 | static const uint8_t size = 48; 421 | static const bool read_only = true; 422 | struct { 423 | float c[3][3]; 424 | float b[3]; 425 | } payload; 426 | }; 427 | 428 | struct VnMagneticGravityReferenceVectors { 429 | static const uint8_t id = 21; 430 | static const uint8_t size = 24; 431 | static const bool read_only = false; 432 | struct { 433 | float mag_ref_x; 434 | float mag_ref_y; 435 | float mag_ref_z; 436 | float acc_ref_x; 437 | float acc_ref_y; 438 | float acc_ref_z; 439 | } payload; 440 | }; 441 | 442 | struct VnReferenceVectorConfiguration { 443 | static const uint8_t id = 83; 444 | /* differs from manual, but confirmed 40 bytes with VN support */ 445 | static const uint8_t size = 40; 446 | static const bool read_only = false; 447 | struct { 448 | uint8_t use_mag_model; 449 | uint8_t use_gravity_model; 450 | uint8_t resv1 = 0; 451 | uint8_t resv2 = 0; 452 | uint32_t recalc_threshold; 453 | float year; 454 | uint8_t padding[4]; 455 | double latitude; 456 | double longitude; 457 | double altitude; 458 | } payload; 459 | }; 460 | 461 | /* VN-100 specific registers */ 462 | 463 | struct VnVpeMagnetometerBasicTuning { 464 | static const uint8_t id = 36; 465 | static const uint8_t size = 36; 466 | static const bool read_only = false; 467 | struct { 468 | float base_tuning_x; 469 | float base_tuning_y; 470 | float base_tuning_z; 471 | float adaptive_tuning_x; 472 | float adaptive_tuning_y; 473 | float adaptive_tuning_z; 474 | float adaptive_filtering_x; 475 | float adaptive_filtering_y; 476 | float adaptive_filtering_z; 477 | } payload; 478 | }; 479 | 480 | struct VnVpeAccelerometerBasicTuning { 481 | static const uint8_t id = 38; 482 | static const uint8_t size = 36; 483 | static const bool read_only = false; 484 | struct { 485 | float base_tuning_x; 486 | float base_tuning_y; 487 | float base_tuning_z; 488 | float adaptive_tuning_x; 489 | float adaptive_tuning_y; 490 | float adaptive_tuning_z; 491 | float adaptive_filtering_x; 492 | float adaptive_filtering_y; 493 | float adaptive_filtering_z; 494 | } payload; 495 | }; 496 | 497 | struct VnFilterStartupGyroBias { 498 | static const uint8_t id = 43; 499 | static const uint8_t size = 12; 500 | static const bool read_only = false; 501 | struct { 502 | float gyro_bias_x; 503 | float gyro_bias_y; 504 | float gyro_bias_z; 505 | } payload; 506 | }; 507 | 508 | struct VnVpeGyroBasicTuning { 509 | static const uint8_t id = 40; 510 | static const uint8_t size = 36; 511 | static const bool read_only = false; 512 | struct { 513 | float base_tuning_x; 514 | float base_tuning_y; 515 | float base_tuning_z; 516 | float adaptive_tuning_x; 517 | float adaptive_tuning_y; 518 | float adaptive_tuning_z; 519 | float adaptive_filtering_x; 520 | float adaptive_filtering_y; 521 | float adaptive_filtering_z; 522 | } payload; 523 | }; 524 | 525 | struct VnVelocityCompensationControl { 526 | static const uint8_t id = 51; 527 | static const uint8_t size = 12; 528 | static const bool read_only = false; 529 | struct { 530 | uint8_t mode; 531 | float velocity_tuning; 532 | float rate_tuning; 533 | } payload; 534 | }; 535 | 536 | struct VnVelocityCompensationMeasurement { 537 | static const uint8_t id = 50; 538 | static const uint8_t size = 12; 539 | static const bool read_only = false; 540 | struct { 541 | float velocity_x; 542 | float velocity_y; 543 | float velocity_z; 544 | } payload; 545 | }; 546 | 547 | /* VN-200 specific registers */ 548 | 549 | struct VnGnssSolutionLla { 550 | static const uint8_t id = 58; 551 | static const uint8_t size = 72; 552 | static const bool read_only = false; 553 | struct { 554 | double time; 555 | uint16_t week; 556 | uint8_t gps_fix; 557 | uint8_t num_sats; 558 | uint8_t padding[4]; 559 | double latitude; 560 | double longitude; 561 | double altitude; 562 | float ned_vel_x; 563 | float ned_vel_y; 564 | float ned_vel_z; 565 | float north_acc; 566 | float east_acc; 567 | float vert_acc; 568 | float speed_acc; 569 | float time_acc; 570 | } payload; 571 | }; 572 | 573 | struct VnGnssSolutionEcef { 574 | static const uint8_t id = 59; 575 | static const uint8_t size = 72; 576 | static const bool read_only = false; 577 | struct { 578 | double time; 579 | uint16_t week; 580 | uint8_t gps_fix; 581 | uint8_t num_sats; 582 | uint8_t padding[4]; 583 | double position_x; 584 | double position_y; 585 | double position_z; 586 | float velocity_x; 587 | float velocity_y; 588 | float velocity_z; 589 | float pos_acc_x; 590 | float pos_acc_y; 591 | float pos_acc_z; 592 | float speed_acc; 593 | float time_acc; 594 | } payload; 595 | }; 596 | 597 | struct VnGnssConfiguration { 598 | static const uint8_t id = 55; 599 | static const uint8_t size = 5; 600 | static const bool read_only = false; 601 | struct { 602 | uint8_t mode; 603 | uint8_t pps_source; 604 | uint8_t rate = 5; 605 | uint8_t time_sync_delta = 0; 606 | uint8_t ant_power = 1; 607 | } payload; 608 | }; 609 | 610 | struct VnGnssAntennaOffset { 611 | static const uint8_t id = 57; 612 | static const uint8_t size = 12; 613 | static const bool read_only = false; 614 | struct { 615 | float position_x; 616 | float position_y; 617 | float position_z; 618 | } payload; 619 | }; 620 | 621 | struct VnInsSolutionLla { 622 | static const uint8_t id = 63; 623 | static const uint8_t size = 72; 624 | static const bool read_only = true; 625 | struct { 626 | double time; 627 | uint16_t week; 628 | uint16_t status; 629 | float yaw; 630 | float pitch; 631 | float roll; 632 | double latitude; 633 | double longitude; 634 | double altitude; 635 | float ned_vel_x; 636 | float ned_vel_y; 637 | float ned_vel_z; 638 | float att_uncertainty; 639 | float pos_uncertainty; 640 | float vel_uncertainty; 641 | } payload; 642 | }; 643 | 644 | struct VnInsSolutionEcef { 645 | static const uint8_t id = 64; 646 | static const uint8_t size = 72; 647 | static const bool read_only = true; 648 | struct { 649 | double time; 650 | uint16_t week; 651 | uint16_t status; 652 | float yaw; 653 | float pitch; 654 | float roll; 655 | double position_x; 656 | double position_y; 657 | double position_z; 658 | float velocity_x; 659 | float velocity_y; 660 | float velocity_z; 661 | float att_uncertainty; 662 | float pos_uncertainty; 663 | float vel_uncertainty; 664 | } payload; 665 | }; 666 | 667 | struct VnInsStateLla { 668 | static const uint8_t id = 72; 669 | static const uint8_t size = 80; 670 | static const bool read_only = true; 671 | struct { 672 | float yaw; 673 | float pitch; 674 | float roll; 675 | uint8_t padding[4]; 676 | double latitude; 677 | double longitude; 678 | double altitude; 679 | float velocity_x; 680 | float velocity_y; 681 | float velocity_z; 682 | float accel_x; 683 | float accel_y; 684 | float accel_z; 685 | float angular_rate_x; 686 | float angular_rate_y; 687 | float angular_rate_z; 688 | } payload; 689 | }; 690 | 691 | struct VnInsStateEcef { 692 | static const uint8_t id = 73; 693 | static const uint8_t size = 80; 694 | static const bool read_only = true; 695 | struct { 696 | float yaw; 697 | float pitch; 698 | float roll; 699 | uint8_t padding[4]; 700 | double position_x; 701 | double position_y; 702 | double position_z; 703 | float velocity_x; 704 | float velocity_y; 705 | float velocity_z; 706 | float accel_x; 707 | float accel_y; 708 | float accel_z; 709 | float angular_rate_x; 710 | float angular_rate_y; 711 | float angular_rate_z; 712 | } payload; 713 | }; 714 | 715 | struct VnStartupFilterBiasEstimate { 716 | static const uint8_t id = 74; 717 | static const uint8_t size = 28; 718 | static const bool read_only = false; 719 | struct { 720 | float gyro_bias_x; 721 | float gyro_bias_y; 722 | float gyro_bias_z; 723 | float accel_bias_x; 724 | float accel_bias_y; 725 | float accel_bias_z; 726 | float pressure_bias; 727 | } payload; 728 | }; 729 | 730 | /* VN-300 specific registers */ 731 | 732 | struct VnGpsCompassBaseline { 733 | static const uint8_t id = 93; 734 | static const uint8_t size = 24; 735 | static const bool read_only = false; 736 | struct { 737 | float position_x; 738 | float position_y; 739 | float position_z; 740 | float uncertainty_x; 741 | float uncertainty_y; 742 | float uncertainty_z; 743 | } payload; 744 | }; 745 | 746 | struct VnGpsCompassEstimatedBaseline { 747 | static const uint8_t id = 97; 748 | static const uint8_t size = 28; 749 | static const bool read_only = true; 750 | struct { 751 | uint8_t estimated_baseline_used; 752 | uint8_t resv1 = 0; 753 | uint16_t num_meas; 754 | float position_x; 755 | float position_y; 756 | float position_z; 757 | float uncertainty_x; 758 | float uncertainty_y; 759 | float uncertainty_z; 760 | } payload; 761 | }; 762 | 763 | struct VnInsBasicConfiguration { 764 | static const uint8_t id = 67; 765 | static const uint8_t size = 4; 766 | static const bool read_only = false; 767 | struct { 768 | uint8_t scenario; 769 | uint8_t ahrs_aiding; 770 | uint8_t est_baseline; 771 | uint8_t resv2 = 0; 772 | } payload; 773 | }; 774 | 775 | } // namespace bfs 776 | 777 | #endif // VECTOR_NAV_SRC_REGISTERS_H_ NOLINT 778 | -------------------------------------------------------------------------------- /src/vector_nav.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Brian R Taylor 3 | * brian.taylor@bolderflight.com 4 | * 5 | * Copyright (c) 2022 Bolder Flight Systems Inc 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the “Software”), to 9 | * deal in the Software without restriction, including without limitation the 10 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | * sell copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in 15 | * all copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | * IN THE SOFTWARE. 24 | */ 25 | 26 | #ifndef VECTOR_NAV_SRC_VECTOR_NAV_H_ // NOLINT 27 | #define VECTOR_NAV_SRC_VECTOR_NAV_H_ 28 | 29 | #if defined(ARDUINO) 30 | #include 31 | #endif 32 | #include "registers.h" // NOLINT 33 | #include "vn.h" // NOLINT 34 | #include "vn100.h" // NOLINT 35 | #include "vn200.h" // NOLINT 36 | #include "vn300.h" // NOLINT 37 | 38 | #endif // VECTOR_NAV_SRC_VECTOR_NAV_H_ NOLINT 39 | -------------------------------------------------------------------------------- /src/vn.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Brian R Taylor 3 | * brian.taylor@bolderflight.com 4 | * 5 | * Copyright (c) 2022 Bolder Flight Systems Inc 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the “Software”), to 9 | * deal in the Software without restriction, including without limitation the 10 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | * sell copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in 15 | * all copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | * IN THE SOFTWARE. 24 | */ 25 | 26 | #ifndef VECTOR_NAV_SRC_VN_H_ // NOLINT 27 | #define VECTOR_NAV_SRC_VN_H_ 28 | 29 | #if defined(ARDUINO) 30 | #include 31 | #include 32 | #include "elapsedMillis.h" 33 | #else 34 | #include "core/core.h" 35 | #endif 36 | 37 | namespace bfs { 38 | 39 | class VectorNav { 40 | public: 41 | enum ErrorCode { 42 | ERROR_SUCCESS = 0, 43 | ERROR_HARD_FAULT = 1, 44 | ERROR_SERIAL_BUFFER_OVERFLOW = 2, 45 | ERROR_INVALID_CHECKSUM = 3, 46 | ERROR_INVALID_COMMAND = 4, 47 | ERROR_NOT_ENOUGH_PARAMETERS = 5, 48 | ERROR_TOO_MANY_PARAMETERS = 6, 49 | ERROR_INVALID_PARAMETER = 7, 50 | ERROR_INVALID_REGISTER = 8, 51 | ERROR_UNAUTHORIZED_ACCESS = 9, 52 | ERROR_WATCHDOG_RESET = 10, 53 | ERROR_OUTPUT_BUFFER_OVERFLOW = 11, 54 | ERROR_INSUFFICIENT_BAUD_RATE = 12, 55 | ERROR_NULL_PTR = 13, 56 | ERROR_NO_COMM = 14, 57 | ERROR_WRONG_MODEL = 15, 58 | ERROR_ERROR_BUFFER_OVERFLOW = 255 59 | }; 60 | VectorNav() {} 61 | VectorNav(SPIClass *bus, const uint8_t cs) : bus_(bus), cs_(cs) {} 62 | void Config(SPIClass *bus, const uint8_t cs) { 63 | bus_ = bus; 64 | cs_ = cs; 65 | } 66 | /* Initialize communication */ 67 | void Init() { 68 | pinMode(cs_, OUTPUT); 69 | #if defined(TEENSYDUINO) 70 | digitalWriteFast(cs_, HIGH); 71 | #else 72 | digitalWrite(cs_, HIGH); 73 | #endif 74 | } 75 | /* Read register */ 76 | template 77 | ErrorCode ReadRegister(REG *ptr) { 78 | static_assert(ptr->size == sizeof(ptr->payload), 79 | "VectorNav register payload size incorrect"); 80 | /* Delay if necessary */ 81 | if (time_since_comm_us_ < WAIT_TIME_US_) { 82 | delayMicroseconds(WAIT_TIME_US_ - time_since_comm_us_); 83 | } 84 | /* Read request */ 85 | bus_->beginTransaction(SPISettings(SPI_CLOCK_, MSBFIRST, SPI_MODE3)); 86 | #if defined(TEENSYDUINO) 87 | digitalWriteFast(cs_, LOW); 88 | #else 89 | digitalWrite(cs_, LOW); 90 | #endif 91 | bus_->transfer(CMD_READ_); 92 | bus_->transfer(ptr->id); 93 | bus_->transfer(0x00); 94 | bus_->transfer(0x00); 95 | #if defined(TEENSYDUINO) 96 | digitalWriteFast(cs_, HIGH); 97 | #else 98 | digitalWrite(cs_, HIGH); 99 | #endif 100 | /* Wait for VectorNav to fill response buffer */ 101 | delayMicroseconds(WAIT_TIME_US_); 102 | /* Read the response buffer header */ 103 | #if defined(TEENSYDUINO) 104 | digitalWriteFast(cs_, LOW); 105 | #else 106 | digitalWrite(cs_, LOW); 107 | #endif 108 | empty_ = bus_->transfer(0x00); 109 | cmd_ = bus_->transfer(0x00); 110 | arg_ = bus_->transfer(0x00); 111 | err_ = bus_->transfer(0x00); 112 | /* Check for errors */ 113 | if (err_ != ERROR_SUCCESS) { 114 | #if defined(TEENSYDUINO) 115 | digitalWriteFast(cs_, HIGH); 116 | #else 117 | digitalWrite(cs_, HIGH); 118 | #endif 119 | bus_->endTransaction(); 120 | time_since_comm_us_ = 0; 121 | return static_cast(err_); 122 | } 123 | /* Read the response data payload */ 124 | for (size_t i = 0; i < sizeof(ptr->payload); i++) { 125 | reinterpret_cast(&ptr->payload)[i] = bus_->transfer(0x00); 126 | } 127 | #if defined(TEENSYDUINO) 128 | digitalWriteFast(cs_, HIGH); 129 | #else 130 | digitalWrite(cs_, HIGH); 131 | #endif 132 | bus_->endTransaction(); 133 | time_since_comm_us_ = 0; 134 | return ERROR_SUCCESS; 135 | } 136 | /* Write register */ 137 | template 138 | ErrorCode WriteRegister(const REG &ref) { 139 | static_assert(ref.size == sizeof(ref.payload), 140 | "VectorNav register payload size incorrect"); 141 | static_assert(ref.read_only == false, 142 | "VectorNav read-only register"); 143 | /* Delay if necessary */ 144 | if (time_since_comm_us_ < WAIT_TIME_US_) { 145 | delayMicroseconds(WAIT_TIME_US_ - time_since_comm_us_); 146 | } 147 | /* Write register */ 148 | bus_->beginTransaction(SPISettings(SPI_CLOCK_, MSBFIRST, SPI_MODE3)); 149 | #if defined(TEENSYDUINO) 150 | digitalWriteFast(cs_, LOW); 151 | #else 152 | digitalWrite(cs_, LOW); 153 | #endif 154 | bus_->transfer(CMD_WRITE_); 155 | bus_->transfer(ref.id); 156 | bus_->transfer(0x00); 157 | bus_->transfer(0x00); 158 | for (size_t i = 0; i < sizeof(ref.payload); i++) { 159 | bus_->transfer(reinterpret_cast(&ref.payload)[i]); 160 | } 161 | #if defined(TEENSYDUINO) 162 | digitalWriteFast(cs_, HIGH); 163 | #else 164 | digitalWrite(cs_, HIGH); 165 | #endif 166 | /* Wait for VectorNav to fill response buffer */ 167 | delayMicroseconds(WAIT_TIME_US_); 168 | /* Read the response buffer header */ 169 | #if defined(TEENSYDUINO) 170 | digitalWriteFast(cs_, LOW); 171 | #else 172 | digitalWrite(cs_, LOW); 173 | #endif 174 | empty_ = bus_->transfer(0x00); 175 | cmd_ = bus_->transfer(0x00); 176 | arg_ = bus_->transfer(0x00); 177 | err_ = bus_->transfer(0x00); 178 | #if defined(TEENSYDUINO) 179 | digitalWriteFast(cs_, HIGH); 180 | #else 181 | digitalWrite(cs_, HIGH); 182 | #endif 183 | bus_->endTransaction(); 184 | time_since_comm_us_ = 0; 185 | return static_cast(err_); 186 | } 187 | /* Write command */ 188 | ErrorCode WriteSettings() { 189 | /* Delay if necessary */ 190 | if (time_since_comm_us_ < WAIT_TIME_US_) { 191 | delayMicroseconds(WAIT_TIME_US_ - time_since_comm_us_); 192 | } 193 | /* Write settings */ 194 | bus_->beginTransaction(SPISettings(SPI_CLOCK_, MSBFIRST, SPI_MODE3)); 195 | #if defined(TEENSYDUINO) 196 | digitalWriteFast(cs_, LOW); 197 | #else 198 | digitalWrite(cs_, LOW); 199 | #endif 200 | bus_->transfer(CMD_WRITE_SETTINGS_); 201 | bus_->transfer(0x00); 202 | bus_->transfer(0x00); 203 | bus_->transfer(0x00); 204 | #if defined(TEENSYDUINO) 205 | digitalWriteFast(cs_, HIGH); 206 | #else 207 | digitalWrite(cs_, HIGH); 208 | #endif 209 | /* Wait for operation to complete */ 210 | delay(1000); 211 | /* Read the response buffer header */ 212 | #if defined(TEENSYDUINO) 213 | digitalWriteFast(cs_, LOW); 214 | #else 215 | digitalWrite(cs_, LOW); 216 | #endif 217 | empty_ = bus_->transfer(0x00); 218 | cmd_ = bus_->transfer(0x00); 219 | arg_ = bus_->transfer(0x00); 220 | err_ = bus_->transfer(0x00); 221 | #if defined(TEENSYDUINO) 222 | digitalWriteFast(cs_, HIGH); 223 | #else 224 | digitalWrite(cs_, HIGH); 225 | #endif 226 | bus_->endTransaction(); 227 | time_since_comm_us_ = 0; 228 | return static_cast(err_); 229 | } 230 | void RestoreFactorySettings() { 231 | /* Delay if necessary */ 232 | if (time_since_comm_us_ < WAIT_TIME_US_) { 233 | delayMicroseconds(WAIT_TIME_US_ - time_since_comm_us_); 234 | } 235 | /* Write register */ 236 | bus_->beginTransaction(SPISettings(SPI_CLOCK_, MSBFIRST, SPI_MODE3)); 237 | #if defined(TEENSYDUINO) 238 | digitalWriteFast(cs_, LOW); 239 | #else 240 | digitalWrite(cs_, LOW); 241 | #endif 242 | bus_->transfer(CMD_RESTORE_FACTORY_SETTINGS_); 243 | bus_->transfer(0x00); 244 | bus_->transfer(0x00); 245 | bus_->transfer(0x00); 246 | #if defined(TEENSYDUINO) 247 | digitalWriteFast(cs_, HIGH); 248 | #else 249 | digitalWrite(cs_, HIGH); 250 | #endif 251 | bus_->endTransaction(); 252 | time_since_comm_us_ = 0; 253 | /* Wait for operation to complete */ 254 | delay(2500); 255 | } 256 | ErrorCode Tare() { 257 | /* Delay if necessary */ 258 | if (time_since_comm_us_ < WAIT_TIME_US_) { 259 | delayMicroseconds(WAIT_TIME_US_ - time_since_comm_us_); 260 | } 261 | /* Write register */ 262 | bus_->beginTransaction(SPISettings(SPI_CLOCK_, MSBFIRST, SPI_MODE3)); 263 | #if defined(TEENSYDUINO) 264 | digitalWriteFast(cs_, LOW); 265 | #else 266 | digitalWrite(cs_, LOW); 267 | #endif 268 | bus_->transfer(CMD_TARE_); 269 | bus_->transfer(0x00); 270 | bus_->transfer(0x00); 271 | bus_->transfer(0x00); 272 | #if defined(TEENSYDUINO) 273 | digitalWriteFast(cs_, HIGH); 274 | #else 275 | digitalWrite(cs_, HIGH); 276 | #endif 277 | /* Wait for VectorNav to fill response buffer */ 278 | delayMicroseconds(WAIT_TIME_US_); 279 | /* Read the response buffer header */ 280 | #if defined(TEENSYDUINO) 281 | digitalWriteFast(cs_, LOW); 282 | #else 283 | digitalWrite(cs_, LOW); 284 | #endif 285 | empty_ = bus_->transfer(0x00); 286 | cmd_ = bus_->transfer(0x00); 287 | arg_ = bus_->transfer(0x00); 288 | err_ = bus_->transfer(0x00); 289 | #if defined(TEENSYDUINO) 290 | digitalWriteFast(cs_, HIGH); 291 | #else 292 | digitalWrite(cs_, HIGH); 293 | #endif 294 | bus_->endTransaction(); 295 | time_since_comm_us_ = 0; 296 | return static_cast(err_); 297 | } 298 | void Reset() { 299 | /* Delay if necessary */ 300 | if (time_since_comm_us_ < WAIT_TIME_US_) { 301 | delayMicroseconds(WAIT_TIME_US_ - time_since_comm_us_); 302 | } 303 | /* Write register */ 304 | bus_->beginTransaction(SPISettings(SPI_CLOCK_, MSBFIRST, SPI_MODE3)); 305 | #if defined(TEENSYDUINO) 306 | digitalWriteFast(cs_, LOW); 307 | #else 308 | digitalWrite(cs_, LOW); 309 | #endif 310 | bus_->transfer(CMD_RESET_); 311 | bus_->transfer(0x00); 312 | bus_->transfer(0x00); 313 | bus_->transfer(0x00); 314 | #if defined(TEENSYDUINO) 315 | digitalWriteFast(cs_, HIGH); 316 | #else 317 | digitalWrite(cs_, HIGH); 318 | #endif 319 | bus_->endTransaction(); 320 | time_since_comm_us_ = 0; 321 | /* Wait for operation to complete */ 322 | delay(2000); 323 | } 324 | ErrorCode KnownMagneticDisturbance(const bool present) { 325 | /* Delay if necessary */ 326 | if (time_since_comm_us_ < WAIT_TIME_US_) { 327 | delayMicroseconds(WAIT_TIME_US_ - time_since_comm_us_); 328 | } 329 | /* Write register */ 330 | bus_->beginTransaction(SPISettings(SPI_CLOCK_, MSBFIRST, SPI_MODE3)); 331 | #if defined(TEENSYDUINO) 332 | digitalWriteFast(cs_, LOW); 333 | #else 334 | digitalWrite(cs_, LOW); 335 | #endif 336 | bus_->transfer(CMD_KNOWN_MAG_DIST_); 337 | bus_->transfer(present); 338 | bus_->transfer(0x00); 339 | bus_->transfer(0x00); 340 | #if defined(TEENSYDUINO) 341 | digitalWriteFast(cs_, HIGH); 342 | #else 343 | digitalWrite(cs_, HIGH); 344 | #endif 345 | /* Wait for VectorNav to fill response buffer */ 346 | delayMicroseconds(WAIT_TIME_US_); 347 | /* Read the response buffer header */ 348 | #if defined(TEENSYDUINO) 349 | digitalWriteFast(cs_, LOW); 350 | #else 351 | digitalWrite(cs_, LOW); 352 | #endif 353 | empty_ = bus_->transfer(0x00); 354 | cmd_ = bus_->transfer(0x00); 355 | arg_ = bus_->transfer(0x00); 356 | err_ = bus_->transfer(0x00); 357 | #if defined(TEENSYDUINO) 358 | digitalWriteFast(cs_, HIGH); 359 | #else 360 | digitalWrite(cs_, HIGH); 361 | #endif 362 | bus_->endTransaction(); 363 | time_since_comm_us_ = 0; 364 | return static_cast(err_); 365 | } 366 | ErrorCode KnownAccelerationDisturbance(const bool present) { 367 | /* Delay if necessary */ 368 | if (time_since_comm_us_ < WAIT_TIME_US_) { 369 | delayMicroseconds(WAIT_TIME_US_ - time_since_comm_us_); 370 | } 371 | /* Write register */ 372 | bus_->beginTransaction(SPISettings(SPI_CLOCK_, MSBFIRST, SPI_MODE3)); 373 | #if defined(TEENSYDUINO) 374 | digitalWriteFast(cs_, LOW); 375 | #else 376 | digitalWrite(cs_, LOW); 377 | #endif 378 | bus_->transfer(CMD_KNOWN_ACCEL_DIST_); 379 | bus_->transfer(present); 380 | bus_->transfer(0x00); 381 | bus_->transfer(0x00); 382 | #if defined(TEENSYDUINO) 383 | digitalWriteFast(cs_, HIGH); 384 | #else 385 | digitalWrite(cs_, HIGH); 386 | #endif 387 | /* Wait for VectorNav to fill response buffer */ 388 | delayMicroseconds(WAIT_TIME_US_); 389 | /* Read the response buffer header */ 390 | #if defined(TEENSYDUINO) 391 | digitalWriteFast(cs_, LOW); 392 | #else 393 | digitalWrite(cs_, LOW); 394 | #endif 395 | empty_ = bus_->transfer(0x00); 396 | cmd_ = bus_->transfer(0x00); 397 | arg_ = bus_->transfer(0x00); 398 | err_ = bus_->transfer(0x00); 399 | #if defined(TEENSYDUINO) 400 | digitalWriteFast(cs_, HIGH); 401 | #else 402 | digitalWrite(cs_, HIGH); 403 | #endif 404 | bus_->endTransaction(); 405 | time_since_comm_us_ = 0; 406 | return static_cast(err_); 407 | } 408 | 409 | ErrorCode SetGyroBias() { 410 | /* Delay if necessary */ 411 | if (time_since_comm_us_ < WAIT_TIME_US_) { 412 | delayMicroseconds(WAIT_TIME_US_ - time_since_comm_us_); 413 | } 414 | /* Write register */ 415 | bus_->beginTransaction(SPISettings(SPI_CLOCK_, MSBFIRST, SPI_MODE3)); 416 | #if defined(TEENSYDUINO) 417 | digitalWriteFast(cs_, LOW); 418 | #else 419 | digitalWrite(cs_, LOW); 420 | #endif 421 | bus_->transfer(CMD_SET_GYRO_BIAS_); 422 | bus_->transfer(0x00); 423 | bus_->transfer(0x00); 424 | bus_->transfer(0x00); 425 | #if defined(TEENSYDUINO) 426 | digitalWriteFast(cs_, HIGH); 427 | #else 428 | digitalWrite(cs_, HIGH); 429 | #endif 430 | /* Wait for VectorNav to fill response buffer */ 431 | delayMicroseconds(WAIT_TIME_US_); 432 | /* Read the response buffer header */ 433 | #if defined(TEENSYDUINO) 434 | digitalWriteFast(cs_, LOW); 435 | #else 436 | digitalWrite(cs_, LOW); 437 | #endif 438 | empty_ = bus_->transfer(0x00); 439 | cmd_ = bus_->transfer(0x00); 440 | arg_ = bus_->transfer(0x00); 441 | err_ = bus_->transfer(0x00); 442 | #if defined(TEENSYDUINO) 443 | digitalWriteFast(cs_, HIGH); 444 | #else 445 | digitalWrite(cs_, HIGH); 446 | #endif 447 | bus_->endTransaction(); 448 | time_since_comm_us_ = 0; 449 | return static_cast(err_); 450 | } 451 | 452 | ErrorCode SetFilterBias() { 453 | /* Delay if necessary */ 454 | if (time_since_comm_us_ < WAIT_TIME_US_) { 455 | delayMicroseconds(WAIT_TIME_US_ - time_since_comm_us_); 456 | } 457 | /* Write register */ 458 | bus_->beginTransaction(SPISettings(SPI_CLOCK_, MSBFIRST, SPI_MODE3)); 459 | #if defined(TEENSYDUINO) 460 | digitalWriteFast(cs_, LOW); 461 | #else 462 | digitalWrite(cs_, LOW); 463 | #endif 464 | bus_->transfer(CMD_SET_FILTER_BIAS_); 465 | bus_->transfer(0x00); 466 | bus_->transfer(0x00); 467 | bus_->transfer(0x00); 468 | #if defined(TEENSYDUINO) 469 | digitalWriteFast(cs_, HIGH); 470 | #else 471 | digitalWrite(cs_, HIGH); 472 | #endif 473 | /* Wait for VectorNav to fill response buffer */ 474 | delayMicroseconds(WAIT_TIME_US_); 475 | /* Read the response buffer header */ 476 | #if defined(TEENSYDUINO) 477 | digitalWriteFast(cs_, LOW); 478 | #else 479 | digitalWrite(cs_, LOW); 480 | #endif 481 | empty_ = bus_->transfer(0x00); 482 | cmd_ = bus_->transfer(0x00); 483 | arg_ = bus_->transfer(0x00); 484 | err_ = bus_->transfer(0x00); 485 | #if defined(TEENSYDUINO) 486 | digitalWriteFast(cs_, HIGH); 487 | #else 488 | digitalWrite(cs_, HIGH); 489 | #endif 490 | bus_->endTransaction(); 491 | time_since_comm_us_ = 0; 492 | return static_cast(err_); 493 | } 494 | 495 | private: 496 | /* SPI */ 497 | SPIClass *bus_; 498 | uint8_t cs_; 499 | static constexpr uint32_t SPI_CLOCK_ = 16000000; 500 | static constexpr uint8_t WAIT_TIME_US_ = 100; 501 | elapsedMicros time_since_comm_us_ = WAIT_TIME_US_; 502 | /* Response header */ 503 | uint8_t cmd_, arg_, empty_, err_; 504 | /* Commands */ 505 | static constexpr uint8_t CMD_READ_ = 0x01; 506 | static constexpr uint8_t CMD_WRITE_ = 0x02; 507 | static constexpr uint8_t CMD_WRITE_SETTINGS_ = 0x03; 508 | static constexpr uint8_t CMD_RESTORE_FACTORY_SETTINGS_ = 0x04; 509 | static constexpr uint8_t CMD_TARE_ = 0x05; 510 | static constexpr uint8_t CMD_RESET_ = 0x06; 511 | static constexpr uint8_t CMD_KNOWN_MAG_DIST_ = 0x08; 512 | static constexpr uint8_t CMD_KNOWN_ACCEL_DIST_ = 0x09; 513 | static constexpr uint8_t CMD_SET_GYRO_BIAS_ = 0x0C; 514 | static constexpr uint8_t CMD_SET_FILTER_BIAS_ = 0x11; 515 | }; 516 | 517 | } // namespace bfs 518 | 519 | #endif // VECTOR_NAV_SRC_VN_H_ NOLINT 520 | -------------------------------------------------------------------------------- /src/vn100.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Brian R Taylor 3 | * brian.taylor@bolderflight.com 4 | * 5 | * Copyright (c) 2022 Bolder Flight Systems Inc 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the “Software”), to 9 | * deal in the Software without restriction, including without limitation the 10 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | * sell copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in 15 | * all copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | * IN THE SOFTWARE. 24 | */ 25 | 26 | #if defined(ARDUINO) 27 | #include 28 | #include 29 | #else 30 | #include "core/core.h" 31 | #endif 32 | #include "vn100.h" // NOLINT 33 | #include "vector_nav.h" // NOLINT 34 | #include "registers.h" // NOLINT 35 | 36 | namespace bfs { 37 | 38 | constexpr char Vn100::PROD_NAME_[]; 39 | 40 | bool Vn100::Begin() { 41 | vn_.Init(); 42 | error_code_ = vn_.ReadRegister(&serial_num_); 43 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 44 | if (serial_num_.payload.serial_num == 0) { 45 | error_code_ = VectorNav::ERROR_NO_COMM; 46 | return false; 47 | } 48 | error_code_ = vn_.ReadRegister(&model_num_); 49 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 50 | for (size_t i = 0; i < sizeof(PROD_NAME_) - 1; i++) { 51 | if (model_num_.payload.product_name[i] != PROD_NAME_[i]) { 52 | error_code_ = VectorNav::ERROR_WRONG_MODEL; 53 | return false; 54 | } 55 | } 56 | return true; 57 | } 58 | 59 | bool Vn100::EnableDrdyInt(const DrdyMode mode, const uint16_t srd) { 60 | error_code_ = vn_.ReadRegister(&sync_cntrl_); 61 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 62 | enum SyncOutPolarity : uint8_t { 63 | NEG_PULSE = 0, 64 | POS_PULSE = 1 65 | }; 66 | sync_cntrl_.payload.sync_out_mode = static_cast(mode); 67 | sync_cntrl_.payload.sync_out_polarity = POS_PULSE; 68 | sync_cntrl_.payload.sync_out_pulse_width = 500000; 69 | sync_cntrl_.payload.sync_out_skip_factor = srd; 70 | error_code_ = vn_.WriteRegister(sync_cntrl_); 71 | return (error_code_ == VectorNav::ERROR_SUCCESS); 72 | } 73 | 74 | bool Vn100::DisableDrdyInt() { 75 | error_code_ = vn_.ReadRegister(&sync_cntrl_); 76 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 77 | sync_cntrl_.payload.sync_out_mode = 0; 78 | error_code_ = vn_.WriteRegister(sync_cntrl_); 79 | return (error_code_ == VectorNav::ERROR_SUCCESS); 80 | } 81 | 82 | bool Vn100::SetMagFilter(const FilterMode mode, const uint16_t window) { 83 | error_code_ = vn_.ReadRegister(&filter_); 84 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 85 | filter_.payload.mag_filter_mode = static_cast(mode); 86 | filter_.payload.mag_window_size = window; 87 | error_code_ = vn_.WriteRegister(filter_); 88 | return (error_code_ == VectorNav::ERROR_SUCCESS); 89 | } 90 | 91 | bool Vn100::GetMagFilter(FilterMode *mode, uint16_t *window) { 92 | if ((!mode) || (!window)) { 93 | error_code_ = VectorNav::ERROR_NULL_PTR; 94 | return false; 95 | } 96 | error_code_ = vn_.ReadRegister(&filter_); 97 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 98 | *mode = static_cast(filter_.payload.mag_filter_mode); 99 | *window = filter_.payload.mag_window_size; 100 | return true; 101 | } 102 | 103 | bool Vn100::SetAccelFilter(const FilterMode mode, const uint16_t window) { 104 | error_code_ = vn_.ReadRegister(&filter_); 105 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 106 | filter_.payload.accel_filter_mode = static_cast(mode); 107 | filter_.payload.accel_window_size = window; 108 | error_code_ = vn_.WriteRegister(filter_); 109 | return (error_code_ == VectorNav::ERROR_SUCCESS); 110 | } 111 | 112 | bool Vn100::GetAccelFilter(FilterMode *mode, uint16_t *window) { 113 | if ((!mode) || (!window)) { 114 | error_code_ = VectorNav::ERROR_NULL_PTR; 115 | return false; 116 | } 117 | error_code_ = vn_.ReadRegister(&filter_); 118 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 119 | *mode = static_cast(filter_.payload.accel_filter_mode); 120 | *window = filter_.payload.accel_window_size; 121 | return true; 122 | } 123 | 124 | bool Vn100::SetGyroFilter(const FilterMode mode, const uint16_t window) { 125 | error_code_ = vn_.ReadRegister(&filter_); 126 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 127 | filter_.payload.gyro_filter_mode = static_cast(mode); 128 | filter_.payload.gyro_window_size = window; 129 | error_code_ = vn_.WriteRegister(filter_); 130 | return (error_code_ == VectorNav::ERROR_SUCCESS); 131 | } 132 | 133 | bool Vn100::GetGyroFilter(FilterMode *mode, uint16_t *window) { 134 | if ((!mode) || (!window)) { 135 | error_code_ = VectorNav::ERROR_NULL_PTR; 136 | return false; 137 | } 138 | error_code_ = vn_.ReadRegister(&filter_); 139 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 140 | *mode = static_cast(filter_.payload.gyro_filter_mode); 141 | *window = filter_.payload.gyro_window_size; 142 | return true; 143 | } 144 | 145 | bool Vn100::SetTemperatureFilter(const FilterMode mode, const uint16_t window) { 146 | error_code_ = vn_.ReadRegister(&filter_); 147 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 148 | filter_.payload.temp_filter_mode = static_cast(mode); 149 | filter_.payload.temp_window_size = window; 150 | error_code_ = vn_.WriteRegister(filter_); 151 | return (error_code_ == VectorNav::ERROR_SUCCESS); 152 | } 153 | 154 | bool Vn100::GetTemperatureFilter(FilterMode *mode, uint16_t *window) { 155 | if ((!mode) || (!window)) { 156 | error_code_ = VectorNav::ERROR_NULL_PTR; 157 | return false; 158 | } 159 | error_code_ = vn_.ReadRegister(&filter_); 160 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 161 | *mode = static_cast(filter_.payload.temp_filter_mode); 162 | *window = filter_.payload.temp_window_size; 163 | return true; 164 | } 165 | 166 | bool Vn100::SetPressureFilter(const FilterMode mode, const uint16_t window) { 167 | error_code_ = vn_.ReadRegister(&filter_); 168 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 169 | filter_.payload.pres_filter_mode = static_cast(mode); 170 | filter_.payload.pres_window_size = window; 171 | error_code_ = vn_.WriteRegister(filter_); 172 | return (error_code_ == VectorNav::ERROR_SUCCESS); 173 | } 174 | 175 | bool Vn100::GetPressureFilter(FilterMode *mode, uint16_t *window) { 176 | if ((!mode) || (!window)) { 177 | error_code_ = VectorNav::ERROR_NULL_PTR; 178 | return false; 179 | } 180 | error_code_ = vn_.ReadRegister(&filter_); 181 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 182 | *mode = static_cast(filter_.payload.pres_filter_mode); 183 | *window = filter_.payload.pres_window_size; 184 | return true; 185 | } 186 | 187 | 188 | bool Vn100::DrdyCallback(const uint8_t int_pin, void (*function)()) { 189 | if (!function) { 190 | error_code_ = VectorNav::ERROR_NULL_PTR; 191 | return false; 192 | } 193 | pinMode(int_pin, INPUT); 194 | attachInterrupt(int_pin, function, RISING); 195 | error_code_ = VectorNav::ERROR_SUCCESS; 196 | return true; 197 | } 198 | 199 | bool Vn100::VelocityCompensation(float speed_mps) { 200 | vel_comp_.payload.velocity_x = speed_mps; 201 | vel_comp_.payload.velocity_y = 0.0f; 202 | vel_comp_.payload.velocity_z = 0.0f; 203 | error_code_ = vn_.WriteRegister(vel_comp_); 204 | return (error_code_ == VectorNav::ERROR_SUCCESS); 205 | } 206 | 207 | bool Vn100::Read() { 208 | error_code_ = vn_.ReadRegister(&attitude_); 209 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 210 | error_code_ = vn_.ReadRegister(&imu_); 211 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 212 | return true; 213 | } 214 | 215 | } // namespace bfs 216 | -------------------------------------------------------------------------------- /src/vn100.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Brian R Taylor 3 | * brian.taylor@bolderflight.com 4 | * 5 | * Copyright (c) 2022 Bolder Flight Systems Inc 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the “Software”), to 9 | * deal in the Software without restriction, including without limitation the 10 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | * sell copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in 15 | * all copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | * IN THE SOFTWARE. 24 | */ 25 | 26 | #ifndef VECTOR_NAV_SRC_VN100_H_ // NOLINT 27 | #define VECTOR_NAV_SRC_VN100_H_ 28 | 29 | #if defined(ARDUINO) 30 | #include 31 | #include 32 | #else 33 | #include 34 | #include 35 | #include "core/core.h" 36 | #endif 37 | #include "vn.h" // NOLINT 38 | #include "registers.h" // NOLINT 39 | 40 | namespace bfs { 41 | 42 | class Vn100 { 43 | public: 44 | enum DrdyMode : uint8_t { 45 | IMU_START = 1, 46 | IMU_READY = 2, 47 | AHRS = 3 48 | }; 49 | enum FilterMode : uint8_t { 50 | FILTER_NONE = 0, 51 | FILTER_UNCOMP_ONLY = 1, 52 | FILTER_COMP_ONLY = 2, 53 | FILTER_BOTH = 3 54 | }; 55 | Vn100() {} 56 | Vn100(SPIClass *bus, const uint8_t cs) : vn_(bus, cs) {} 57 | void Config(SPIClass *bus, const uint8_t cs) {vn_.Config(bus, cs);} 58 | bool Begin(); 59 | bool EnableDrdyInt(const DrdyMode mode, const uint16_t srd); 60 | bool DisableDrdyInt(); 61 | template 62 | bool ApplyRotation(const float (&c)[M][N]) { 63 | static_assert(M == 3, "Expecting 3 x 3 matrix"); 64 | static_assert(N == 3, "Expecting 3 x 3 matrix"); 65 | for (int8_t m = 0; m < M; m++) { 66 | for (int8_t n = 0; n < N; n++) { 67 | rotation_.payload.c[m][n] = c[m][n]; 68 | } 69 | } 70 | error_code_ = vn_.WriteRegister(rotation_); 71 | vn_.WriteSettings(); 72 | vn_.Reset(); 73 | return (error_code_ == VectorNav::ERROR_SUCCESS); 74 | } 75 | template 76 | bool GetRotation(float (&c)[M][N]) { 77 | static_assert(M == 3, "Expecting 3 x 3 matrix"); 78 | static_assert(N == 3, "Expecting 3 x 3 matrix"); 79 | error_code_ = vn_.ReadRegister(&rotation_); 80 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 81 | for (int8_t m = 0; m < M; m++) { 82 | for (int8_t n = 0; n < N; n++) { 83 | c[m][n] = rotation_.payload.c[m][n]; 84 | } 85 | } 86 | return true; 87 | } 88 | bool SetMagFilter(const FilterMode mode, const uint16_t window); 89 | bool GetMagFilter(FilterMode *mode, uint16_t *window); 90 | bool SetAccelFilter(const FilterMode mode, const uint16_t window); 91 | bool GetAccelFilter(FilterMode *mode, uint16_t *window); 92 | bool SetGyroFilter(const FilterMode mode, const uint16_t window); 93 | bool GetGyroFilter(FilterMode *mode, uint16_t *window); 94 | bool SetTemperatureFilter(const FilterMode mode, const uint16_t window); 95 | bool GetTemperatureFilter(FilterMode *mode, uint16_t *window); 96 | bool SetPressureFilter(const FilterMode mode, const uint16_t window); 97 | bool GetPressureFilter(FilterMode *mode, uint16_t *window); 98 | bool DrdyCallback(const uint8_t int_pin, void (*function)()); 99 | bool VelocityCompensation(const float speed_mps); 100 | bool Read(); 101 | inline VectorNav::ErrorCode error_code() {return error_code_;} 102 | 103 | /* Commands */ 104 | bool WriteSettings() { 105 | return (vn_.WriteSettings() == VectorNav::ERROR_SUCCESS); 106 | } 107 | void RestoreFactorySettings() { 108 | vn_.RestoreFactorySettings(); 109 | } 110 | void Reset() { 111 | vn_.Reset(); 112 | } 113 | bool Tare() { 114 | return (vn_.Tare() == VectorNav::ERROR_SUCCESS); 115 | } 116 | bool KnownMagneticDisturbance(const bool present) { 117 | return (vn_.KnownMagneticDisturbance(present) == VectorNav::ERROR_SUCCESS); 118 | } 119 | bool KnownAccelerationDisturbance(const bool present) { 120 | return (vn_.KnownAccelerationDisturbance(present) == 121 | VectorNav::ERROR_SUCCESS); 122 | } 123 | bool SetGyroBias() { 124 | return (vn_.SetGyroBias() == VectorNav::ERROR_SUCCESS); 125 | } 126 | 127 | /* Data */ 128 | inline float yaw_rad() const { 129 | return attitude_.payload.yaw * DEG2RAD_; 130 | } 131 | inline float pitch_rad() const { 132 | return attitude_.payload.pitch * DEG2RAD_; 133 | } 134 | inline float roll_rad() const { 135 | return attitude_.payload.roll * DEG2RAD_; 136 | } 137 | inline float accel_x_mps2() const { 138 | return attitude_.payload.accel_x; 139 | } 140 | inline float accel_y_mps2() const { 141 | return attitude_.payload.accel_y; 142 | } 143 | inline float accel_z_mps2() const { 144 | return attitude_.payload.accel_z; 145 | } 146 | inline float gyro_x_radps() const { 147 | return attitude_.payload.gyro_x; 148 | } 149 | inline float gyro_y_radps() const { 150 | return attitude_.payload.gyro_y; 151 | } 152 | inline float gyro_z_radps() const { 153 | return attitude_.payload.gyro_z; 154 | } 155 | inline float mag_x_ut() const { 156 | return attitude_.payload.mag_x * 100.0f; 157 | } 158 | inline float mag_y_ut() const { 159 | return attitude_.payload.mag_y * 100.0f; 160 | } 161 | inline float mag_z_ut() const { 162 | return attitude_.payload.mag_z * 100.0f; 163 | } 164 | inline float uncomp_accel_x_mps2() const { 165 | return imu_.payload.accel_x; 166 | } 167 | inline float uncomp_accel_y_mps2() const { 168 | return imu_.payload.accel_y; 169 | } 170 | inline float uncomp_accel_z_mps2() const { 171 | return imu_.payload.accel_z; 172 | } 173 | inline float uncomp_gyro_x_radps() const { 174 | return imu_.payload.gyro_x; 175 | } 176 | inline float uncomp_gyro_y_radps() const { 177 | return imu_.payload.gyro_y; 178 | } 179 | inline float uncomp_gyro_z_radps() const { 180 | return imu_.payload.gyro_z; 181 | } 182 | inline float uncomp_mag_x_ut() const { 183 | return imu_.payload.mag_x * 100.0f; 184 | } 185 | inline float uncomp_mag_y_ut() const { 186 | return imu_.payload.mag_y * 100.0f; 187 | } 188 | inline float uncomp_mag_z_ut() const { 189 | return imu_.payload.mag_z * 100.0f; 190 | } 191 | inline float die_temp_c() const { 192 | return imu_.payload.temp; 193 | } 194 | inline float pres_pa() const { 195 | return imu_.payload.pressure * 1000.0f; 196 | } 197 | 198 | private: 199 | /* Register reading and writing */ 200 | VectorNav vn_; 201 | /* Expected product name */ 202 | static constexpr char PROD_NAME_[] = {"VN-100"}; 203 | /* Data */ 204 | static constexpr float DEG2RAD_ = 3.14159265358979323846264338327950288f / 205 | 180.0f; 206 | /* Registers */ 207 | VectorNav::ErrorCode error_code_; 208 | VnModelNumber model_num_; 209 | VnSerialNumber serial_num_; 210 | VnSynchronizationControl sync_cntrl_; 211 | VnReferenceFrameRotation rotation_; 212 | VnImuFilteringConfiguration filter_; 213 | VnVelocityCompensationMeasurement vel_comp_; 214 | VnYawPitchRollMagneticAccelerationAngularRates attitude_; 215 | VnImuMeasurements imu_; 216 | }; 217 | 218 | } // namespace bfs 219 | 220 | #endif // VECTOR_NAV_SRC_VN100_H_ NOLINT 221 | -------------------------------------------------------------------------------- /src/vn200.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Brian R Taylor 3 | * brian.taylor@bolderflight.com 4 | * 5 | * Copyright (c) 2022 Bolder Flight Systems Inc 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the “Software”), to 9 | * deal in the Software without restriction, including without limitation the 10 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | * sell copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in 15 | * all copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | * IN THE SOFTWARE. 24 | */ 25 | 26 | #if defined(ARDUINO) 27 | #include 28 | #include 29 | #else 30 | #include "core/core.h" 31 | #endif 32 | #include "vn200.h" // NOLINT 33 | #include "vector_nav.h" // NOLINT 34 | #include "registers.h" // NOLINT 35 | 36 | namespace bfs { 37 | 38 | constexpr char Vn200::PROD_NAME_[]; 39 | 40 | bool Vn200::Begin() { 41 | vn_.Init(); 42 | error_code_ = vn_.ReadRegister(&serial_num_); 43 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 44 | if (serial_num_.payload.serial_num == 0) { 45 | error_code_ = VectorNav::ERROR_NO_COMM; 46 | return false; 47 | } 48 | error_code_ = vn_.ReadRegister(&model_num_); 49 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 50 | for (size_t i = 0; i < sizeof(PROD_NAME_) - 1; i++) { 51 | if (model_num_.payload.product_name[i] != PROD_NAME_[i]) { 52 | error_code_ = VectorNav::ERROR_WRONG_MODEL; 53 | return false; 54 | } 55 | } 56 | return true; 57 | } 58 | 59 | bool Vn200::EnableDrdyInt(const DrdyMode mode, const uint16_t srd) { 60 | error_code_ = vn_.ReadRegister(&sync_cntrl_); 61 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 62 | enum SyncOutPolarity : uint8_t { 63 | NEG_PULSE = 0, 64 | POS_PULSE = 1 65 | }; 66 | sync_cntrl_.payload.sync_out_mode = static_cast(mode); 67 | sync_cntrl_.payload.sync_out_polarity = POS_PULSE; 68 | sync_cntrl_.payload.sync_out_pulse_width = 500000; 69 | sync_cntrl_.payload.sync_out_skip_factor = srd; 70 | error_code_ = vn_.WriteRegister(sync_cntrl_); 71 | return (error_code_ == VectorNav::ERROR_SUCCESS); 72 | } 73 | 74 | bool Vn200::DisableDrdyInt() { 75 | error_code_ = vn_.ReadRegister(&sync_cntrl_); 76 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 77 | sync_cntrl_.payload.sync_out_mode = 0; 78 | error_code_ = vn_.WriteRegister(sync_cntrl_); 79 | return (error_code_ == VectorNav::ERROR_SUCCESS); 80 | } 81 | 82 | bool Vn200::EnableExternalGnss(const PpsSource pps) { 83 | gnss_config_.payload.mode = 1; // external GNSS 84 | gnss_config_.payload.pps_source = static_cast(pps); 85 | error_code_ = vn_.WriteRegister(gnss_config_); 86 | return (error_code_ == VectorNav::ERROR_SUCCESS); 87 | } 88 | 89 | bool Vn200::DisableExternalGnss() { 90 | gnss_config_.payload.mode = 0; // internal GNSS 91 | gnss_config_.payload.pps_source = 0; 92 | error_code_ = vn_.WriteRegister(gnss_config_); 93 | return (error_code_ == VectorNav::ERROR_SUCCESS); 94 | } 95 | 96 | bool Vn200::SetMagFilter(const FilterMode mode, const uint16_t window) { 97 | error_code_ = vn_.ReadRegister(&filter_); 98 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 99 | filter_.payload.mag_filter_mode = static_cast(mode); 100 | filter_.payload.mag_window_size = window; 101 | error_code_ = vn_.WriteRegister(filter_); 102 | return (error_code_ == VectorNav::ERROR_SUCCESS); 103 | } 104 | 105 | bool Vn200::GetMagFilter(FilterMode *mode, uint16_t *window) { 106 | if ((!mode) || (!window)) { 107 | error_code_ = VectorNav::ERROR_NULL_PTR; 108 | return false; 109 | } 110 | error_code_ = vn_.ReadRegister(&filter_); 111 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 112 | *mode = static_cast(filter_.payload.mag_filter_mode); 113 | *window = filter_.payload.mag_window_size; 114 | return true; 115 | } 116 | 117 | bool Vn200::SetAccelFilter(const FilterMode mode, const uint16_t window) { 118 | error_code_ = vn_.ReadRegister(&filter_); 119 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 120 | filter_.payload.accel_filter_mode = static_cast(mode); 121 | filter_.payload.accel_window_size = window; 122 | error_code_ = vn_.WriteRegister(filter_); 123 | return (error_code_ == VectorNav::ERROR_SUCCESS); 124 | } 125 | 126 | bool Vn200::GetAccelFilter(FilterMode *mode, uint16_t *window) { 127 | if ((!mode) || (!window)) { 128 | error_code_ = VectorNav::ERROR_NULL_PTR; 129 | return false; 130 | } 131 | error_code_ = vn_.ReadRegister(&filter_); 132 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 133 | *mode = static_cast(filter_.payload.accel_filter_mode); 134 | *window = filter_.payload.accel_window_size; 135 | return true; 136 | } 137 | 138 | bool Vn200::SetGyroFilter(const FilterMode mode, const uint16_t window) { 139 | error_code_ = vn_.ReadRegister(&filter_); 140 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 141 | filter_.payload.gyro_filter_mode = static_cast(mode); 142 | filter_.payload.gyro_window_size = window; 143 | error_code_ = vn_.WriteRegister(filter_); 144 | return (error_code_ == VectorNav::ERROR_SUCCESS); 145 | } 146 | 147 | bool Vn200::GetGyroFilter(FilterMode *mode, uint16_t *window) { 148 | if ((!mode) || (!window)) { 149 | error_code_ = VectorNav::ERROR_NULL_PTR; 150 | return false; 151 | } 152 | error_code_ = vn_.ReadRegister(&filter_); 153 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 154 | *mode = static_cast(filter_.payload.gyro_filter_mode); 155 | *window = filter_.payload.gyro_window_size; 156 | return true; 157 | } 158 | 159 | bool Vn200::SetTemperatureFilter(const FilterMode mode, const uint16_t window) { 160 | error_code_ = vn_.ReadRegister(&filter_); 161 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 162 | filter_.payload.temp_filter_mode = static_cast(mode); 163 | filter_.payload.temp_window_size = window; 164 | error_code_ = vn_.WriteRegister(filter_); 165 | return (error_code_ == VectorNav::ERROR_SUCCESS); 166 | } 167 | 168 | bool Vn200::GetTemperatureFilter(FilterMode *mode, uint16_t *window) { 169 | if ((!mode) || (!window)) { 170 | error_code_ = VectorNav::ERROR_NULL_PTR; 171 | return false; 172 | } 173 | error_code_ = vn_.ReadRegister(&filter_); 174 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 175 | *mode = static_cast(filter_.payload.temp_filter_mode); 176 | *window = filter_.payload.temp_window_size; 177 | return true; 178 | } 179 | 180 | bool Vn200::SetPressureFilter(const FilterMode mode, const uint16_t window) { 181 | error_code_ = vn_.ReadRegister(&filter_); 182 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 183 | filter_.payload.pres_filter_mode = static_cast(mode); 184 | filter_.payload.pres_window_size = window; 185 | error_code_ = vn_.WriteRegister(filter_); 186 | return (error_code_ == VectorNav::ERROR_SUCCESS); 187 | } 188 | 189 | bool Vn200::GetPressureFilter(FilterMode *mode, uint16_t *window) { 190 | if ((!mode) || (!window)) { 191 | error_code_ = VectorNav::ERROR_NULL_PTR; 192 | return false; 193 | } 194 | error_code_ = vn_.ReadRegister(&filter_); 195 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 196 | *mode = static_cast(filter_.payload.pres_filter_mode); 197 | *window = filter_.payload.pres_window_size; 198 | return true; 199 | } 200 | 201 | bool Vn200::DrdyCallback(const uint8_t int_pin, void (*function)()) { 202 | if (!function) { 203 | error_code_ = VectorNav::ERROR_NULL_PTR; 204 | return false; 205 | } 206 | pinMode(int_pin, INPUT); 207 | attachInterrupt(int_pin, function, RISING); 208 | error_code_ = VectorNav::ERROR_SUCCESS; 209 | return true; 210 | } 211 | 212 | bool Vn200::Read() { 213 | error_code_ = vn_.ReadRegister(&ins_); 214 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 215 | error_code_ = vn_.ReadRegister(&gnss_); 216 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 217 | error_code_ = vn_.ReadRegister(&comp_imu_); 218 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 219 | error_code_ = vn_.ReadRegister(&uncomp_imu_); 220 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 221 | /* INS status parsing */ 222 | ins_status_buff_[0] = ins_.payload.status & 0xFF; 223 | ins_status_buff_[1] = ins_.payload.status >> 8 & 0xFF; 224 | ins_mode_ = static_cast(ins_status_buff_[0] & 0x03); 225 | ins_gnss_fix_ = ins_status_buff_[0] & 0x04; 226 | ins_time_error_ = ins_status_buff_[0] & 0x08; 227 | ins_imu_error_ = ins_status_buff_[0] & 0x10; 228 | ins_mag_press_error_ = ins_status_buff_[0] & 0x20; 229 | ins_gnss_error_ = ins_status_buff_[0] & 0x40; 230 | ins_error_ = ins_time_error_ || ins_imu_error_ || 231 | ins_mag_press_error_ || ins_gnss_error_; 232 | return true; 233 | } 234 | 235 | bool Vn200::SendExternalGnssData(const VnGnssSolutionLla &ref) { 236 | error_code_ = vn_.WriteRegister(ref); 237 | return (error_code_ == VectorNav::ERROR_SUCCESS); 238 | } 239 | 240 | bool Vn200::SendExternalGnssData(const VnGnssSolutionEcef &ref) { 241 | error_code_ = vn_.WriteRegister(ref); 242 | return (error_code_ == VectorNav::ERROR_SUCCESS); 243 | } 244 | 245 | } // namespace bfs 246 | -------------------------------------------------------------------------------- /src/vn200.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Brian R Taylor 3 | * brian.taylor@bolderflight.com 4 | * 5 | * Copyright (c) 2022 Bolder Flight Systems Inc 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the “Software”), to 9 | * deal in the Software without restriction, including without limitation the 10 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | * sell copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in 15 | * all copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | * IN THE SOFTWARE. 24 | */ 25 | 26 | #ifndef VECTOR_NAV_SRC_VN200_H_ // NOLINT 27 | #define VECTOR_NAV_SRC_VN200_H_ 28 | 29 | #if defined(ARDUINO) 30 | #include 31 | #include 32 | #else 33 | #include 34 | #include 35 | #include "core/core.h" 36 | #endif 37 | #include "vn.h" // NOLINT 38 | #include "registers.h" // NOLINT 39 | 40 | namespace bfs { 41 | 42 | class Vn200 { 43 | public: 44 | enum DrdyMode : uint8_t { 45 | IMU_START = 1, 46 | IMU_READY = 2, 47 | INS = 3, 48 | GNSS_PPS = 6 49 | }; 50 | enum FilterMode : uint8_t { 51 | FILTER_NONE = 0, 52 | FILTER_UNCOMP_ONLY = 1, 53 | FILTER_COMP_ONLY = 2, 54 | FILTER_BOTH = 3 55 | }; 56 | enum InsMode : uint8_t { 57 | NOT_TRACKING = 0, 58 | DEGRADED = 1, 59 | HEALTHY = 2, 60 | GNSS_LOSS = 3 61 | }; 62 | enum GnssFix : uint8_t { 63 | FIX_NONE = 0, 64 | FIX_TIME_ONLY = 1, 65 | FIX_2D = 2, 66 | FIX_3D = 3, 67 | FIX_SBAS = 4 68 | }; 69 | enum PpsSource : uint8_t { 70 | PPS_RISING = 0, 71 | PPS_FALLING = 1, 72 | SYNC_IN_RISING = 2, 73 | SYNC_IN_FALLING = 3 74 | }; 75 | Vn200() {} 76 | Vn200(SPIClass *bus, const uint8_t cs) : vn_(bus, cs) {} 77 | void Config(SPIClass *bus, const uint8_t cs) {vn_.Config(bus, cs);} 78 | bool Begin(); 79 | bool EnableDrdyInt(const DrdyMode mode, const uint16_t srd); 80 | bool DisableDrdyInt(); 81 | bool EnableExternalGnss(const PpsSource pps); 82 | bool DisableExternalGnss(); 83 | template 84 | bool ApplyRotation(const float (&c)[M][N]) { 85 | static_assert(M == 3, "Expecting 3 x 3 matrix"); 86 | static_assert(N == 3, "Expecting 3 x 3 matrix"); 87 | for (int8_t m = 0; m < M; m++) { 88 | for (int8_t n = 0; n < N; n++) { 89 | rotation_.payload.c[m][n] = c[m][n]; 90 | } 91 | } 92 | error_code_ = vn_.WriteRegister(rotation_); 93 | vn_.WriteSettings(); 94 | vn_.Reset(); 95 | return (error_code_ == VectorNav::ERROR_SUCCESS); 96 | } 97 | template 98 | bool GetRotation(float (&c)[M][N]) { 99 | static_assert(M == 3, "Expecting 3 x 3 matrix"); 100 | static_assert(N == 3, "Expecting 3 x 3 matrix"); 101 | error_code_ = vn_.ReadRegister(&rotation_); 102 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 103 | for (int8_t m = 0; m < M; m++) { 104 | for (int8_t n = 0; n < N; n++) { 105 | c[m][n] = rotation_.payload.c[m][n]; 106 | } 107 | } 108 | return true; 109 | } 110 | template 111 | bool SetAntennaOffset(const float (&b)[M]) { 112 | static_assert(M == 3, "Expecting 3 x 1 vector"); 113 | antenna_.payload.position_x = b[0]; 114 | antenna_.payload.position_y = b[1]; 115 | antenna_.payload.position_z = b[2]; 116 | error_code_ = vn_.WriteRegister(antenna_); 117 | return (error_code_ == VectorNav::ERROR_SUCCESS); 118 | } 119 | template 120 | bool GetAntennaOffset(float (&b)[M]) { 121 | static_assert(M == 3, "Expecting 3 x 1 vector"); 122 | error_code_ = vn_.ReadRegister(&antenna_); 123 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 124 | b[0] = antenna_.payload.position_x; 125 | b[1] = antenna_.payload.position_y; 126 | b[2] = antenna_.payload.position_z; 127 | return true; 128 | } 129 | bool SetMagFilter(const FilterMode mode, const uint16_t window); 130 | bool GetMagFilter(FilterMode *mode, uint16_t *window); 131 | bool SetAccelFilter(const FilterMode mode, const uint16_t window); 132 | bool GetAccelFilter(FilterMode *mode, uint16_t *window); 133 | bool SetGyroFilter(const FilterMode mode, const uint16_t window); 134 | bool GetGyroFilter(FilterMode *mode, uint16_t *window); 135 | bool SetTemperatureFilter(const FilterMode mode, const uint16_t window); 136 | bool GetTemperatureFilter(FilterMode *mode, uint16_t *window); 137 | bool SetPressureFilter(const FilterMode mode, const uint16_t window); 138 | bool GetPressureFilter(FilterMode *mode, uint16_t *window); 139 | bool DrdyCallback(const uint8_t int_pin, void (*function)()); 140 | bool Read(); 141 | inline VectorNav::ErrorCode error_code() {return error_code_;} 142 | bool SendExternalGnssData(const VnGnssSolutionLla &ref); 143 | bool SendExternalGnssData(const VnGnssSolutionEcef &ref); 144 | 145 | /* Commands */ 146 | bool WriteSettings() { 147 | return vn_.WriteSettings(); 148 | } 149 | void RestoreFactorySettings() { 150 | vn_.RestoreFactorySettings(); 151 | } 152 | void Reset() { 153 | vn_.Reset(); 154 | } 155 | bool SetFilterBias() { 156 | return vn_.SetFilterBias(); 157 | } 158 | bool KnownMagneticDisturbance(const bool present) { 159 | return vn_.KnownMagneticDisturbance(present); 160 | } 161 | bool KnownAccelerationDisturbance(const bool present) { 162 | return vn_.KnownAccelerationDisturbance(present); 163 | } 164 | bool SetGyroBias() { 165 | return vn_.SetGyroBias(); 166 | } 167 | 168 | /* Data */ 169 | inline InsMode ins_mode() const { 170 | return ins_mode_; 171 | } 172 | inline bool ins_error() const { 173 | return ins_error_; 174 | } 175 | inline bool ins_time_error() const { 176 | return ins_time_error_; 177 | } 178 | inline bool ins_imu_error() const { 179 | return ins_imu_error_; 180 | } 181 | inline bool ins_mag_pres_error() const { 182 | return ins_mag_press_error_; 183 | } 184 | inline bool ins_gnss_error() const { 185 | return ins_gnss_error_; 186 | } 187 | inline double ins_time_s() const { 188 | return ins_.payload.time; 189 | } 190 | inline uint16_t ins_week() const { 191 | return ins_.payload.week; 192 | } 193 | inline float yaw_rad() const { 194 | return ins_.payload.yaw * DEG2RADf_; 195 | } 196 | inline float pitch_rad() const { 197 | return ins_.payload.pitch * DEG2RADf_; 198 | } 199 | inline float roll_rad() const { 200 | return ins_.payload.roll * DEG2RADf_; 201 | } 202 | inline double ins_lat_rad() const { 203 | return ins_.payload.latitude * DEG2RADl_; 204 | } 205 | inline double ins_lon_rad() const { 206 | return ins_.payload.longitude * DEG2RADl_; 207 | } 208 | inline double ins_alt_m() const { 209 | return ins_.payload.altitude; 210 | } 211 | inline float ins_north_vel_mps() const { 212 | return ins_.payload.ned_vel_x; 213 | } 214 | inline float ins_east_vel_mps() const { 215 | return ins_.payload.ned_vel_y; 216 | } 217 | inline float ins_down_vel_mps() const { 218 | return ins_.payload.ned_vel_z; 219 | } 220 | inline float ins_att_uncertainty_rad() const { 221 | return ins_.payload.att_uncertainty * DEG2RADf_; 222 | } 223 | inline float ins_pos_uncertainty_m() const { 224 | return ins_.payload.pos_uncertainty; 225 | } 226 | inline float ins_vel_uncertainty_mps() const { 227 | return ins_.payload.vel_uncertainty; 228 | } 229 | inline double gnss_time_s() const { 230 | return gnss_.payload.time; 231 | } 232 | inline uint16_t gnss_week() const { 233 | return gnss_.payload.week; 234 | } 235 | inline GnssFix gnss_fix() const { 236 | return static_cast(gnss_.payload.gps_fix); 237 | } 238 | inline uint8_t gnss_num_sats() const { 239 | return gnss_.payload.num_sats; 240 | } 241 | inline double gnss_lat_rad() const { 242 | return gnss_.payload.latitude * DEG2RADl_; 243 | } 244 | inline double gnss_lon_rad() const { 245 | return gnss_.payload.longitude * DEG2RADl_; 246 | } 247 | inline double gnss_alt_m() const { 248 | return gnss_.payload.altitude; 249 | } 250 | inline float gnss_north_vel_mps() const { 251 | return gnss_.payload.ned_vel_x; 252 | } 253 | inline float gnss_east_vel_mps() const { 254 | return gnss_.payload.ned_vel_y; 255 | } 256 | inline float gnss_down_vel_mps() const { 257 | return gnss_.payload.ned_vel_z; 258 | } 259 | inline float gnss_north_acc_m() const { 260 | return gnss_.payload.north_acc; 261 | } 262 | inline float gnss_east_acc_m() const { 263 | return gnss_.payload.east_acc; 264 | } 265 | inline float gnss_down_acc_m() const { 266 | return gnss_.payload.vert_acc; 267 | } 268 | inline float gnss_speed_acc_mps() const { 269 | return gnss_.payload.speed_acc; 270 | } 271 | inline float gnss_time_acc_s() const { 272 | return gnss_.payload.time_acc; 273 | } 274 | inline float accel_x_mps2() const { 275 | return comp_imu_.payload.accel_x; 276 | } 277 | inline float accel_y_mps2() const { 278 | return comp_imu_.payload.accel_y; 279 | } 280 | inline float accel_z_mps2() const { 281 | return comp_imu_.payload.accel_z; 282 | } 283 | inline float gyro_x_radps() const { 284 | return comp_imu_.payload.gyro_x; 285 | } 286 | inline float gyro_y_radps() const { 287 | return comp_imu_.payload.gyro_y; 288 | } 289 | inline float gyro_z_radps() const { 290 | return comp_imu_.payload.gyro_z; 291 | } 292 | inline float mag_x_ut() const { 293 | return comp_imu_.payload.mag_x * 100.0f; 294 | } 295 | inline float mag_y_ut() const { 296 | return comp_imu_.payload.mag_y * 100.0f; 297 | } 298 | inline float mag_z_ut() const { 299 | return comp_imu_.payload.mag_z * 100.0f; 300 | } 301 | inline float uncomp_accel_x_mps2() const { 302 | return uncomp_imu_.payload.accel_x; 303 | } 304 | inline float uncomp_accel_y_mps2() const { 305 | return uncomp_imu_.payload.accel_y; 306 | } 307 | inline float uncomp_accel_z_mps2() const { 308 | return uncomp_imu_.payload.accel_z; 309 | } 310 | inline float uncomp_gyro_x_radps() const { 311 | return uncomp_imu_.payload.gyro_x; 312 | } 313 | inline float uncomp_gyro_y_radps() const { 314 | return uncomp_imu_.payload.gyro_y; 315 | } 316 | inline float uncomp_gyro_z_radps() const { 317 | return uncomp_imu_.payload.gyro_z; 318 | } 319 | inline float uncomp_mag_x_ut() const { 320 | return uncomp_imu_.payload.mag_x * 100.0f; 321 | } 322 | inline float uncomp_mag_y_ut() const { 323 | return uncomp_imu_.payload.mag_y * 100.0f; 324 | } 325 | inline float uncomp_mag_z_ut() const { 326 | return uncomp_imu_.payload.mag_z * 100.0f; 327 | } 328 | inline float die_temp_c() const { 329 | return uncomp_imu_.payload.temp; 330 | } 331 | inline float pres_pa() const { 332 | return uncomp_imu_.payload.pressure * 1000.0f; 333 | } 334 | 335 | private: 336 | /* Register reading and writing */ 337 | VectorNav vn_; 338 | /* Expected product name */ 339 | static constexpr char PROD_NAME_[] = {"VN-200"}; 340 | /* Data */ 341 | uint8_t ins_status_buff_[2]; 342 | InsMode ins_mode_; 343 | bool ins_gnss_fix_; 344 | bool ins_error_; 345 | bool ins_time_error_; 346 | bool ins_imu_error_; 347 | bool ins_mag_press_error_; 348 | bool ins_gnss_error_; 349 | static constexpr float DEG2RADf_ = 3.14159265358979323846264338327950288f / 350 | 180.0f; 351 | static constexpr double DEG2RADl_ = 3.14159265358979323846264338327950288 / 352 | 180.0; 353 | /* Registers */ 354 | VectorNav::ErrorCode error_code_; 355 | VnModelNumber model_num_; 356 | VnSerialNumber serial_num_; 357 | VnSynchronizationControl sync_cntrl_; 358 | VnReferenceFrameRotation rotation_; 359 | VnGnssConfiguration gnss_config_; 360 | VnGnssAntennaOffset antenna_; 361 | VnImuFilteringConfiguration filter_; 362 | VnInsSolutionLla ins_; 363 | VnGnssSolutionLla gnss_; 364 | VnMagneticAccelerationAngularRates comp_imu_; 365 | VnImuMeasurements uncomp_imu_; 366 | }; 367 | 368 | } // namespace bfs 369 | 370 | #endif // VECTOR_NAV_SRC_VN200_H_ NOLINT 371 | -------------------------------------------------------------------------------- /src/vn300.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Brian R Taylor 3 | * brian.taylor@bolderflight.com 4 | * 5 | * Copyright (c) 2022 Bolder Flight Systems Inc 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the “Software”), to 9 | * deal in the Software without restriction, including without limitation the 10 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | * sell copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in 15 | * all copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | * IN THE SOFTWARE. 24 | */ 25 | 26 | #if defined(ARDUINO) 27 | #include 28 | #include 29 | #else 30 | #include "core/core.h" 31 | #endif 32 | #include "vn300.h" // NOLINT 33 | #include "vector_nav.h" // NOLINT 34 | #include "registers.h" // NOLINT 35 | 36 | namespace bfs { 37 | 38 | constexpr char Vn300::PROD_NAME_[]; 39 | 40 | bool Vn300::Begin() { 41 | vn_.Init(); 42 | error_code_ = vn_.ReadRegister(&serial_num_); 43 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 44 | if (serial_num_.payload.serial_num == 0) { 45 | error_code_ = VectorNav::ERROR_NO_COMM; 46 | return false; 47 | } 48 | error_code_ = vn_.ReadRegister(&model_num_); 49 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 50 | for (size_t i = 0; i < sizeof(PROD_NAME_) - 1; i++) { 51 | if (model_num_.payload.product_name[i] != PROD_NAME_[i]) { 52 | error_code_ = VectorNav::ERROR_WRONG_MODEL; 53 | return false; 54 | } 55 | } 56 | return true; 57 | } 58 | 59 | bool Vn300::EnableDrdyInt(const DrdyMode mode, const uint16_t srd) { 60 | error_code_ = vn_.ReadRegister(&sync_cntrl_); 61 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 62 | enum SyncOutPolarity : uint8_t { 63 | NEG_PULSE = 0, 64 | POS_PULSE = 1 65 | }; 66 | sync_cntrl_.payload.sync_out_mode = static_cast(mode); 67 | sync_cntrl_.payload.sync_out_polarity = POS_PULSE; 68 | sync_cntrl_.payload.sync_out_pulse_width = 500000; 69 | sync_cntrl_.payload.sync_out_skip_factor = srd; 70 | error_code_ = vn_.WriteRegister(sync_cntrl_); 71 | return (error_code_ == VectorNav::ERROR_SUCCESS); 72 | } 73 | 74 | bool Vn300::DisableDrdyInt() { 75 | error_code_ = vn_.ReadRegister(&sync_cntrl_); 76 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 77 | sync_cntrl_.payload.sync_out_mode = 0; 78 | error_code_ = vn_.WriteRegister(sync_cntrl_); 79 | return (error_code_ == VectorNav::ERROR_SUCCESS); 80 | } 81 | 82 | bool Vn300::SetMagFilter(const FilterMode mode, const uint16_t window) { 83 | error_code_ = vn_.ReadRegister(&filter_); 84 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 85 | filter_.payload.mag_filter_mode = static_cast(mode); 86 | filter_.payload.mag_window_size = window; 87 | error_code_ = vn_.WriteRegister(filter_); 88 | return (error_code_ == VectorNav::ERROR_SUCCESS); 89 | } 90 | 91 | bool Vn300::GetMagFilter(FilterMode *mode, uint16_t *window) { 92 | if ((!mode) || (!window)) { 93 | error_code_ = VectorNav::ERROR_NULL_PTR; 94 | return false; 95 | } 96 | error_code_ = vn_.ReadRegister(&filter_); 97 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 98 | *mode = static_cast(filter_.payload.mag_filter_mode); 99 | *window = filter_.payload.mag_window_size; 100 | return true; 101 | } 102 | 103 | bool Vn300::SetAccelFilter(const FilterMode mode, const uint16_t window) { 104 | error_code_ = vn_.ReadRegister(&filter_); 105 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 106 | filter_.payload.accel_filter_mode = static_cast(mode); 107 | filter_.payload.accel_window_size = window; 108 | error_code_ = vn_.WriteRegister(filter_); 109 | return (error_code_ == VectorNav::ERROR_SUCCESS); 110 | } 111 | 112 | bool Vn300::GetAccelFilter(FilterMode *mode, uint16_t *window) { 113 | if ((!mode) || (!window)) { 114 | error_code_ = VectorNav::ERROR_NULL_PTR; 115 | return false; 116 | } 117 | error_code_ = vn_.ReadRegister(&filter_); 118 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 119 | *mode = static_cast(filter_.payload.accel_filter_mode); 120 | *window = filter_.payload.accel_window_size; 121 | return true; 122 | } 123 | 124 | bool Vn300::SetGyroFilter(const FilterMode mode, const uint16_t window) { 125 | error_code_ = vn_.ReadRegister(&filter_); 126 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 127 | filter_.payload.gyro_filter_mode = static_cast(mode); 128 | filter_.payload.gyro_window_size = window; 129 | error_code_ = vn_.WriteRegister(filter_); 130 | return (error_code_ == VectorNav::ERROR_SUCCESS); 131 | } 132 | 133 | bool Vn300::GetGyroFilter(FilterMode *mode, uint16_t *window) { 134 | if ((!mode) || (!window)) { 135 | error_code_ = VectorNav::ERROR_NULL_PTR; 136 | return false; 137 | } 138 | error_code_ = vn_.ReadRegister(&filter_); 139 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 140 | *mode = static_cast(filter_.payload.gyro_filter_mode); 141 | *window = filter_.payload.gyro_window_size; 142 | return true; 143 | } 144 | 145 | bool Vn300::SetTemperatureFilter(const FilterMode mode, const uint16_t window) { 146 | error_code_ = vn_.ReadRegister(&filter_); 147 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 148 | filter_.payload.temp_filter_mode = static_cast(mode); 149 | filter_.payload.temp_window_size = window; 150 | error_code_ = vn_.WriteRegister(filter_); 151 | return (error_code_ == VectorNav::ERROR_SUCCESS); 152 | } 153 | 154 | bool Vn300::GetTemperatureFilter(FilterMode *mode, uint16_t *window) { 155 | if ((!mode) || (!window)) { 156 | error_code_ = VectorNav::ERROR_NULL_PTR; 157 | return false; 158 | } 159 | error_code_ = vn_.ReadRegister(&filter_); 160 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 161 | *mode = static_cast(filter_.payload.temp_filter_mode); 162 | *window = filter_.payload.temp_window_size; 163 | return true; 164 | } 165 | 166 | bool Vn300::SetPressureFilter(const FilterMode mode, const uint16_t window) { 167 | error_code_ = vn_.ReadRegister(&filter_); 168 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 169 | filter_.payload.pres_filter_mode = static_cast(mode); 170 | filter_.payload.pres_window_size = window; 171 | error_code_ = vn_.WriteRegister(filter_); 172 | return (error_code_ == VectorNav::ERROR_SUCCESS); 173 | } 174 | 175 | bool Vn300::GetPressureFilter(FilterMode *mode, uint16_t *window) { 176 | if ((!mode) || (!window)) { 177 | error_code_ = VectorNav::ERROR_NULL_PTR; 178 | return false; 179 | } 180 | error_code_ = vn_.ReadRegister(&filter_); 181 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 182 | *mode = static_cast(filter_.payload.pres_filter_mode); 183 | *window = filter_.payload.pres_window_size; 184 | return true; 185 | } 186 | 187 | bool Vn300::DrdyCallback(const uint8_t int_pin, void (*function)()) { 188 | if (!function) { 189 | error_code_ = VectorNav::ERROR_NULL_PTR; 190 | return false; 191 | } 192 | pinMode(int_pin, INPUT); 193 | attachInterrupt(int_pin, function, RISING); 194 | error_code_ = VectorNav::ERROR_SUCCESS; 195 | return true; 196 | } 197 | 198 | bool Vn300::Read() { 199 | error_code_ = vn_.ReadRegister(&ins_); 200 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 201 | error_code_ = vn_.ReadRegister(&gnss_); 202 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 203 | error_code_ = vn_.ReadRegister(&comp_imu_); 204 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 205 | error_code_ = vn_.ReadRegister(&uncomp_imu_); 206 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 207 | /* INS status parsing */ 208 | ins_status_buff_[0] = ins_.payload.status & 0xFF; 209 | ins_status_buff_[1] = ins_.payload.status >> 8 & 0xFF; 210 | ins_mode_ = static_cast(ins_status_buff_[0] & 0x03); 211 | ins_gnss_fix_ = ins_status_buff_[0] & 0x04; 212 | ins_time_error_ = ins_status_buff_[0] & 0x08; 213 | ins_imu_error_ = ins_status_buff_[0] & 0x10; 214 | ins_mag_press_error_ = ins_status_buff_[0] & 0x20; 215 | ins_gnss_error_ = ins_status_buff_[0] & 0x40; 216 | ins_error_ = ins_time_error_ || ins_imu_error_ || 217 | ins_mag_press_error_ || ins_gnss_error_; 218 | ins_gnss_heading_ = ins_status_buff_[1] & 0x01; 219 | ins_gnss_compass_ = ins_status_buff_[1] & 0x02; 220 | return true; 221 | } 222 | 223 | } // namespace bfs 224 | -------------------------------------------------------------------------------- /src/vn300.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Brian R Taylor 3 | * brian.taylor@bolderflight.com 4 | * 5 | * Copyright (c) 2022 Bolder Flight Systems Inc 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the “Software”), to 9 | * deal in the Software without restriction, including without limitation the 10 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | * sell copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in 15 | * all copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | * IN THE SOFTWARE. 24 | */ 25 | 26 | #ifndef VECTOR_NAV_SRC_VN300_H_ // NOLINT 27 | #define VECTOR_NAV_SRC_VN300_H_ 28 | 29 | #if defined(ARDUINO) 30 | #include 31 | #include 32 | #else 33 | #include 34 | #include 35 | #include "core/core.h" 36 | #endif 37 | #include "vn.h" // NOLINT 38 | #include "registers.h" // NOLINT 39 | 40 | namespace bfs { 41 | 42 | class Vn300 { 43 | public: 44 | enum DrdyMode : uint8_t { 45 | IMU_START = 1, 46 | IMU_READY = 2, 47 | INS = 3, 48 | GNSS_PPS = 6 49 | }; 50 | enum FilterMode : uint8_t { 51 | FILTER_NONE = 0, 52 | FILTER_UNCOMP_ONLY = 1, 53 | FILTER_COMP_ONLY = 2, 54 | FILTER_BOTH = 3 55 | }; 56 | enum InsMode : uint8_t { 57 | NOT_TRACKING = 0, 58 | DEGRADED = 1, 59 | HEALTHY = 2, 60 | GNSS_LOSS = 3 61 | }; 62 | enum GnssFix : uint8_t { 63 | FIX_NONE = 0, 64 | FIX_TIME_ONLY = 1, 65 | FIX_2D = 2, 66 | FIX_3D = 3, 67 | FIX_SBAS = 4 68 | }; 69 | Vn300() {} 70 | Vn300(SPIClass *bus, const uint8_t cs) : vn_(bus, cs) {} 71 | void Config(SPIClass *bus, const uint8_t cs) {vn_.Config(bus, cs);} 72 | bool Begin(); 73 | bool EnableDrdyInt(const DrdyMode mode, const uint16_t srd); 74 | bool DisableDrdyInt(); 75 | template 76 | bool ApplyRotation(const float (&c)[M][N]) { 77 | static_assert(M == 3, "Expecting 3 x 3 matrix"); 78 | static_assert(N == 3, "Expecting 3 x 3 matrix"); 79 | for (int8_t m = 0; m < M; m++) { 80 | for (int8_t n = 0; n < N; n++) { 81 | rotation_.payload.c[m][n] = c[m][n]; 82 | } 83 | } 84 | error_code_ = vn_.WriteRegister(rotation_); 85 | vn_.WriteSettings(); 86 | vn_.Reset(); 87 | return (error_code_ == VectorNav::ERROR_SUCCESS); 88 | } 89 | template 90 | bool GetRotation(float (&c)[M][N]) { 91 | static_assert(M == 3, "Expecting 3 x 3 matrix"); 92 | static_assert(N == 3, "Expecting 3 x 3 matrix"); 93 | error_code_ = vn_.ReadRegister(&rotation_); 94 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 95 | for (int8_t m = 0; m < M; m++) { 96 | for (int8_t n = 0; n < N; n++) { 97 | c[m][n] = rotation_.payload.c[m][n]; 98 | } 99 | } 100 | return true; 101 | } 102 | template 103 | bool SetAntennaOffset(const float (&b)[M]) { 104 | static_assert(M == 3, "Expecting 3 x 1 vector"); 105 | antenna_.payload.position_x = b[0]; 106 | antenna_.payload.position_y = b[1]; 107 | antenna_.payload.position_z = b[2]; 108 | error_code_ = vn_.WriteRegister(antenna_); 109 | return (error_code_ == VectorNav::ERROR_SUCCESS); 110 | } 111 | template 112 | bool GetAntennaOffset(float (&b)[M]) { 113 | static_assert(M == 3, "Expecting 3 x 1 vector"); 114 | error_code_ = vn_.ReadRegister(&antenna_); 115 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 116 | b[0] = antenna_.payload.position_x; 117 | b[1] = antenna_.payload.position_y; 118 | b[2] = antenna_.payload.position_z; 119 | return true; 120 | } 121 | template 122 | bool SetCompassBaseline(const float (&pos)[M], 123 | const float (&uncert)[M]) { 124 | static_assert(M == 3, "Expecting 3 x 1 vector"); 125 | baseline_.payload.position_x = pos[0]; 126 | baseline_.payload.position_y = pos[1]; 127 | baseline_.payload.position_z = pos[2]; 128 | baseline_.payload.uncertainty_x = uncert[0]; 129 | baseline_.payload.uncertainty_y = uncert[1]; 130 | baseline_.payload.uncertainty_z = uncert[2]; 131 | error_code_ = vn_.WriteRegister(baseline_); 132 | return (error_code_ == VectorNav::ERROR_SUCCESS); 133 | } 134 | template 135 | bool GetCompassBaseline(float (&pos)[M], float (&uncert)[M]) { 136 | static_assert(M == 3, "Expecting 3 x 1 vector"); 137 | error_code_ = vn_.ReadRegister(&baseline_); 138 | if (error_code_ != VectorNav::ERROR_SUCCESS) {return false;} 139 | pos[0] = baseline_.payload.position_x; 140 | pos[1] = baseline_.payload.position_y; 141 | pos[2] = baseline_.payload.position_z; 142 | uncert[0] = baseline_.payload.uncertainty_x; 143 | uncert[1] = baseline_.payload.uncertainty_y; 144 | uncert[2] = baseline_.payload.uncertainty_z; 145 | return true; 146 | } 147 | bool SetMagFilter(const FilterMode mode, const uint16_t window); 148 | bool GetMagFilter(FilterMode *mode, uint16_t *window); 149 | bool SetAccelFilter(const FilterMode mode, const uint16_t window); 150 | bool GetAccelFilter(FilterMode *mode, uint16_t *window); 151 | bool SetGyroFilter(const FilterMode mode, const uint16_t window); 152 | bool GetGyroFilter(FilterMode *mode, uint16_t *window); 153 | bool SetTemperatureFilter(const FilterMode mode, const uint16_t window); 154 | bool GetTemperatureFilter(FilterMode *mode, uint16_t *window); 155 | bool SetPressureFilter(const FilterMode mode, const uint16_t window); 156 | bool GetPressureFilter(FilterMode *mode, uint16_t *window); 157 | bool DrdyCallback(const uint8_t int_pin, void (*function)()); 158 | bool Read(); 159 | inline VectorNav::ErrorCode error_code() {return error_code_;} 160 | 161 | /* Commands */ 162 | bool WriteSettings() { 163 | return vn_.WriteSettings(); 164 | } 165 | void RestoreFactorySettings() { 166 | vn_.RestoreFactorySettings(); 167 | } 168 | void Reset() { 169 | vn_.Reset(); 170 | } 171 | bool SetFilterBias() { 172 | return vn_.SetFilterBias(); 173 | } 174 | bool KnownMagneticDisturbance(const bool present) { 175 | return vn_.KnownMagneticDisturbance(present); 176 | } 177 | bool KnownAccelerationDisturbance(const bool present) { 178 | return vn_.KnownAccelerationDisturbance(present); 179 | } 180 | bool SetGyroBias() { 181 | return vn_.SetGyroBias(); 182 | } 183 | 184 | /* Data */ 185 | inline InsMode ins_mode() const { 186 | return ins_mode_; 187 | } 188 | inline bool ins_error() const { 189 | return ins_error_; 190 | } 191 | inline bool ins_time_error() const { 192 | return ins_time_error_; 193 | } 194 | inline bool ins_imu_error() const { 195 | return ins_imu_error_; 196 | } 197 | inline bool ins_mag_pres_error() const { 198 | return ins_mag_press_error_; 199 | } 200 | inline bool ins_gnss_error() const { 201 | return ins_gnss_error_; 202 | } 203 | inline bool ins_gnss_heading() const { 204 | return ins_gnss_heading_; 205 | } 206 | inline bool ins_gnss_compass() const { 207 | return ins_gnss_compass_; 208 | } 209 | inline double ins_time_s() const { 210 | return ins_.payload.time; 211 | } 212 | inline uint16_t ins_week() const { 213 | return ins_.payload.week; 214 | } 215 | inline float yaw_rad() const { 216 | return ins_.payload.yaw * DEG2RADf_; 217 | } 218 | inline float pitch_rad() const { 219 | return ins_.payload.pitch * DEG2RADf_; 220 | } 221 | inline float roll_rad() const { 222 | return ins_.payload.roll * DEG2RADf_; 223 | } 224 | inline double ins_lat_rad() const { 225 | return ins_.payload.latitude * DEG2RADl_; 226 | } 227 | inline double ins_lon_rad() const { 228 | return ins_.payload.longitude * DEG2RADl_; 229 | } 230 | inline double ins_alt_m() const { 231 | return ins_.payload.altitude; 232 | } 233 | inline float ins_north_vel_mps() const { 234 | return ins_.payload.ned_vel_x; 235 | } 236 | inline float ins_east_vel_mps() const { 237 | return ins_.payload.ned_vel_y; 238 | } 239 | inline float ins_down_vel_mps() const { 240 | return ins_.payload.ned_vel_z; 241 | } 242 | inline float ins_att_uncertainty_rad() const { 243 | return ins_.payload.att_uncertainty * DEG2RADf_; 244 | } 245 | inline float ins_pos_uncertainty_m() const { 246 | return ins_.payload.pos_uncertainty; 247 | } 248 | inline float ins_vel_uncertainty_mps() const { 249 | return ins_.payload.vel_uncertainty; 250 | } 251 | inline double gnss_time_s() const { 252 | return gnss_.payload.time; 253 | } 254 | inline uint16_t gnss_week() const { 255 | return gnss_.payload.week; 256 | } 257 | inline GnssFix gnss_fix() const { 258 | return static_cast(gnss_.payload.gps_fix); 259 | } 260 | inline uint8_t gnss_num_sats() const { 261 | return gnss_.payload.num_sats; 262 | } 263 | inline double gnss_lat_rad() const { 264 | return gnss_.payload.latitude * DEG2RADl_; 265 | } 266 | inline double gnss_lon_rad() const { 267 | return gnss_.payload.longitude * DEG2RADl_; 268 | } 269 | inline double gnss_alt_m() const { 270 | return gnss_.payload.altitude; 271 | } 272 | inline float gnss_north_vel_mps() const { 273 | return gnss_.payload.ned_vel_x; 274 | } 275 | inline float gnss_east_vel_mps() const { 276 | return gnss_.payload.ned_vel_y; 277 | } 278 | inline float gnss_down_vel_mps() const { 279 | return gnss_.payload.ned_vel_z; 280 | } 281 | inline float gnss_north_acc_m() const { 282 | return gnss_.payload.north_acc; 283 | } 284 | inline float gnss_east_acc_m() const { 285 | return gnss_.payload.east_acc; 286 | } 287 | inline float gnss_down_acc_m() const { 288 | return gnss_.payload.vert_acc; 289 | } 290 | inline float gnss_speed_acc_mps() const { 291 | return gnss_.payload.speed_acc; 292 | } 293 | inline float gnss_time_acc_s() const { 294 | return gnss_.payload.time_acc; 295 | } 296 | inline float accel_x_mps2() const { 297 | return comp_imu_.payload.accel_x; 298 | } 299 | inline float accel_y_mps2() const { 300 | return comp_imu_.payload.accel_y; 301 | } 302 | inline float accel_z_mps2() const { 303 | return comp_imu_.payload.accel_z; 304 | } 305 | inline float gyro_x_radps() const { 306 | return comp_imu_.payload.gyro_x; 307 | } 308 | inline float gyro_y_radps() const { 309 | return comp_imu_.payload.gyro_y; 310 | } 311 | inline float gyro_z_radps() const { 312 | return comp_imu_.payload.gyro_z; 313 | } 314 | inline float mag_x_ut() const { 315 | return comp_imu_.payload.mag_x * 100.0f; 316 | } 317 | inline float mag_y_ut() const { 318 | return comp_imu_.payload.mag_y * 100.0f; 319 | } 320 | inline float mag_z_ut() const { 321 | return comp_imu_.payload.mag_z * 100.0f; 322 | } 323 | inline float uncomp_accel_x_mps2() const { 324 | return uncomp_imu_.payload.accel_x; 325 | } 326 | inline float uncomp_accel_y_mps2() const { 327 | return uncomp_imu_.payload.accel_y; 328 | } 329 | inline float uncomp_accel_z_mps2() const { 330 | return uncomp_imu_.payload.accel_z; 331 | } 332 | inline float uncomp_gyro_x_radps() const { 333 | return uncomp_imu_.payload.gyro_x; 334 | } 335 | inline float uncomp_gyro_y_radps() const { 336 | return uncomp_imu_.payload.gyro_y; 337 | } 338 | inline float uncomp_gyro_z_radps() const { 339 | return uncomp_imu_.payload.gyro_z; 340 | } 341 | inline float uncomp_mag_x_ut() const { 342 | return uncomp_imu_.payload.mag_x * 100.0f; 343 | } 344 | inline float uncomp_mag_y_ut() const { 345 | return uncomp_imu_.payload.mag_y * 100.0f; 346 | } 347 | inline float uncomp_mag_z_ut() const { 348 | return uncomp_imu_.payload.mag_z * 100.0f; 349 | } 350 | inline float die_temp_c() const { 351 | return uncomp_imu_.payload.temp; 352 | } 353 | inline float pres_pa() const { 354 | return uncomp_imu_.payload.pressure * 1000.0f; 355 | } 356 | 357 | private: 358 | /* Register reading and writing */ 359 | VectorNav vn_; 360 | /* Expected product name */ 361 | static constexpr char PROD_NAME_[] = {"VN-300"}; 362 | /* Data */ 363 | uint8_t ins_status_buff_[2]; 364 | InsMode ins_mode_; 365 | bool ins_gnss_fix_; 366 | bool ins_error_; 367 | bool ins_time_error_; 368 | bool ins_imu_error_; 369 | bool ins_mag_press_error_; 370 | bool ins_gnss_error_; 371 | bool ins_gnss_heading_; 372 | bool ins_gnss_compass_; 373 | static constexpr float DEG2RADf_ = 3.14159265358979323846264338327950288f / 374 | 180.0f; 375 | static constexpr double DEG2RADl_ = 3.14159265358979323846264338327950288 / 376 | 180.0; 377 | /* Registers */ 378 | VectorNav::ErrorCode error_code_; 379 | VnModelNumber model_num_; 380 | VnSerialNumber serial_num_; 381 | VnSynchronizationControl sync_cntrl_; 382 | VnReferenceFrameRotation rotation_; 383 | VnGnssAntennaOffset antenna_; 384 | VnGpsCompassBaseline baseline_; 385 | VnImuFilteringConfiguration filter_; 386 | VnInsSolutionLla ins_; 387 | VnGnssSolutionLla gnss_; 388 | VnMagneticAccelerationAngularRates comp_imu_; 389 | VnImuMeasurements uncomp_imu_; 390 | }; 391 | 392 | } // namespace bfs 393 | 394 | #endif // VECTOR_NAV_SRC_VN300_H_ NOLINT 395 | --------------------------------------------------------------------------------