├── .github └── workflows │ └── rust.yml ├── .gitignore ├── Cargo.toml ├── LICENSE ├── README.md ├── bno055.jpg ├── examples └── calibrate.rs ├── fuzz ├── .gitignore ├── Cargo.toml └── fuzz_targets │ ├── calibrate.rs │ └── init.rs └── src ├── acc_config.rs ├── lib.rs ├── regs.rs └── std.rs /.github/workflows/rust.yml: -------------------------------------------------------------------------------- 1 | name: Rust 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | 9 | env: 10 | CARGO_TERM_COLOR: always 11 | 12 | jobs: 13 | lints-and-checks: 14 | name: Lints and checks 15 | runs-on: ubuntu-latest 16 | steps: 17 | - uses: actions/checkout@v4 18 | with: 19 | submodules: true 20 | - uses: dtolnay/rust-toolchain@nightly 21 | with: 22 | components: rustfmt, clippy, rust-docs 23 | 24 | - name: Rustfmt lints 25 | run: cargo fmt --all -- --check 26 | 27 | - name: Clippy lints 28 | run: cargo clippy --no-deps -- -D warnings 29 | 30 | - name: Build docs 31 | run: RUSTDOCFLAGS="--cfg docsrs" cargo +nightly doc --all-features --no-deps 32 | 33 | build: 34 | runs-on: ubuntu-latest 35 | strategy: 36 | matrix: 37 | rust: 38 | - stable 39 | - beta 40 | - nightly 41 | - "1.65" # MSRV 42 | 43 | steps: 44 | - name: Checkout Sources 45 | uses: actions/checkout@v4 46 | - name: Install Rust (thumbv7em) 47 | uses: dtolnay/rust-toolchain@master 48 | with: 49 | toolchain: ${{ matrix.rust }} 50 | target: thumbv7em-none-eabihf 51 | - name: Build - default features 52 | run: cargo build 53 | - name: Build - std 54 | run: cargo build --features "std" 55 | - name: Build - serde 56 | run: cargo build --features "serde" 57 | - name: Build - defmt-03 58 | run: cargo build --features "defmt-03" 59 | - name: Build - examples 60 | run: cargo build --examples 61 | - name: Run tests 62 | run: cargo test 63 | - name: Install cargo-fuzz 64 | # pre-build binaries 65 | uses: taiki-e/install-action@v2 66 | with: 67 | tool: cargo-fuzz 68 | - name: Fuzz (nightly only) 69 | if: ${{ matrix.rust == 'nightly' }} 70 | run: | 71 | cargo +nightly fuzz run init -- -max_total_time=900 72 | cargo +nightly fuzz run calibrate -- -max_total_time=1800 73 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | target 2 | Cargo.lock 3 | bno055.iml 4 | .idea 5 | -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "bno055" 3 | description = "Bosch Sensortec BNO055 9-axis IMU driver" 4 | version = "0.4.0" # Remember to update lib.rs 5 | authors = ["Eugene P. ", "Henrik B. "] 6 | repository = "https://github.com/eupn/bno055" 7 | edition = "2018" 8 | categories = [ 9 | "embedded", 10 | "hardware-support", 11 | "no-std", 12 | ] 13 | keywords = [ 14 | "embedded-hal-driver", 15 | "bno055", 16 | ] 17 | license-file = "LICENSE" 18 | readme = "README.md" 19 | 20 | # linux-embedded-hal requires 1.65 because of nix 21 | rust-version = "1.65" 22 | 23 | [dependencies] 24 | byteorder = { version = "1", default-features = false } 25 | serde = { version = "1.0", default-features = false, features = ["derive"], optional = true } 26 | mint = "^0.5.4" 27 | bitflags = "2" 28 | num-traits = { version = "0.2.15", default-features = false, features = ["libm"] } 29 | num-derive = "0.4.1" 30 | 31 | defmt = { version = "0.3", optional = true } 32 | 33 | embedded-hal = { version = "1.0" } 34 | 35 | [features] 36 | default = [] 37 | std = [] 38 | 39 | defmt-03 = ["dep:defmt", "embedded-hal/defmt-03"] 40 | serde = ["dep:serde", "mint/serde"] 41 | 42 | [dev-dependencies] 43 | linux-embedded-hal = "0.4" 44 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 eupn 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 all 13 | 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 THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## Bosch Sensortec BNO055 embedded-hal driver 2 | 3 | [![](https://img.shields.io/crates/v/bno055.svg?style=flat)](https://crates.io/crates/bno055) 4 | [![docs.rs](https://docs.rs/bno055/badge.svg)](https://docs.rs/bno055/) 5 | [![](https://img.shields.io/crates/d/bno055.svg?maxAge=3600)](https://crates.io/crates/bno055) 6 | 7 | 8 | 9 | ## What is this? 10 | 11 | This is a [embedded-hal](https://github.com/rust-embedded/embedded-hal) driver 12 | for Bosch's Absolute Orientation Sensor [BNO055](https://ae-bst.resource.bosch.com/media/_tech/media/datasheets/BST-BNO055-DS000.pdf). 13 | 14 | It is device-agnostic and uses embedded-hal's `Write`/`WriteRead` (for I2C) 15 | and `Delay` traits for its operation. 16 | 17 | Uses and re-exports [mint](https://crates.io/crates/mint)'s 18 | [Quaternion](https://docs.rs/mint/0.5.1/mint/struct.Quaternion.html) for quaternion reading 19 | and [EulerAngles](https://docs.rs/mint/0.5.1/mint/struct.EulerAngles.html) for Euler angles 20 | and [Vector3](https://docs.rs/mint/0.5.1/mint/struct.Vector3.html) for sensor readings. 21 | 22 | ## Usage notes 23 | ### Important note on I2C issues 24 | As [noted e.g. by Adafruit](https://learn.adafruit.com/adafruit-bno055-absolute-orientation-sensor) the sensor has issues 25 | with its I2C implementation, which causes it to not work correctly with certain microcontrollers. 26 | 27 | This seems to be caused by clock stretching, thus running at lower I2C speeds and with increased I2C timeouts should 28 | resolve the issue. 29 | 30 | ### Initial startup delay 31 | The sensor has an initial startup time during which interaction with it will fail. 32 | As per [the documentation](https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf) 33 | this is in the 400ms - 650ms range (consult chapter 1.2 / page 14 for further details). 34 | If your microcontroller is faster in starting up you might have to delay before talking to the sensor (or retry on failure). 35 | 36 | ## Feature flags 37 | 38 | ### `std` 39 | 40 | By default, this crate is `no_std` compatible. However, you can enable `std` features by enabling the `std` feature flag. 41 | At the moment this only adds `std::error::Error` trait implementation for the `Error` type. 42 | 43 | ### `serde` 44 | 45 | The `serde` flag adds implementation of `Serialize` / `Deserialize` to `BNO055Calibration`. 46 | 47 | **Note:** `serde` itself is `no_std` compatible, however not all serializers are (e.g. `serde-json` is not but `serde-json-core` is), 48 | so be careful that you're not enabling `serde`'s `std` feature by accident (see [here](https://serde.rs/no-std.html#no-std-support) for a complete explanation). 49 | 50 | ## Usage 51 | 52 | 1. Add a dependency to `Cargo.toml`: 53 | 54 | ```bash 55 | cargo add bno055 56 | ``` 57 | 58 | 2. Instantiate and init the device: 59 | 60 | ```rust 61 | // ... declare and configure your I2c and Delay implementations ... 62 | // let i2c = ...; 63 | // let delay = ...; 64 | 65 | // Init BNO055 IMU 66 | let mut imu = bno055::Bno055::new(i2c); 67 | 68 | imu.init(&mut delay)?; 69 | 70 | // Enable 9-degrees-of-freedom sensor fusion mode with fast magnetometer calibration 71 | imu.set_mode(bno055::BNO055OperationMode::NDOF, &mut delay)?; 72 | 73 | Ok(imu) 74 | ``` 75 | 76 | 3. Read orientation data, quaternion or euler angles (roll, pitch, yaw/heading): 77 | 78 | ```rust 79 | let quat: mint::Quaternion = imu.quaternion()?; 80 | // or: 81 | let euler: mint::EulerAngles = imu.euler_angles()?; 82 | ``` 83 | 84 | >Due to the BNO055 firmware bugs, the Euler angles reading shouldn't be relied on. 85 | I recommend to stick with quaternion readings and convert them to the Euler angles later if needed. 86 | 87 | ## Details and examples 88 | 89 | ### Device calibration 90 | 91 | To calibrate the device's sensors for first time: 92 | 93 | ```rust 94 | use bno055::{BNO055Calibration, BNO055OperationMode, BNO055_CALIB_SIZE}; 95 | 96 | let bno055 = ...; 97 | 98 | // Enter NDOF (absolute orientation) sensor fusion mode which is also performing 99 | // a regular sensors calibration 100 | bno055.set_mode(BNO055OperationMode::NDOF)?; 101 | 102 | // Wait for device to auto-calibrate. 103 | // Please perform steps necessary for auto-calibration to kick in. 104 | // Required steps are described in Datasheet section 3.11 105 | while !bno055.is_fully_calibrated() {} 106 | 107 | let calib = bno055.calibration_profile()?; 108 | 109 | // Save calibration profile in NVRAM 110 | mcu.nvram_write(BNO055_CALIB_ADDR, calib.as_bytes(), BNO055_CALIB_SIZE)?; 111 | ``` 112 | 113 | To load a previously saved calibration profile: 114 | 115 | ```rust 116 | use bno055::{BNO055Calibration, BNO055OperationMode, BNO055_CALIB_SIZE}; 117 | 118 | let bno055 = ...; 119 | 120 | // Read saved calibration profile from MCUs NVRAM 121 | let mut buf = [0u8; BNO055_CALIB_SIZE]; 122 | mcu.nvram_read(BNO055_CALIB_ADDR, &mut buf, BNO055_CALIB_SIZE)?; 123 | 124 | // Apply calibration profile 125 | let calib = BNO055Calibration::from_buf(buf); 126 | bno055.set_calibration_profile(calib)?; 127 | ``` 128 | 129 | ### Remapping of axes to correspond your mounting 130 | 131 | BNO055 allows to change default axes to meet the chip orientation with 132 | an actual physical device orientation, thus providing possibility to place BNO055 133 | chip on PCB as suitable for the designer and to match the chip's axes to physical 134 | axes in software later. 135 | 136 | ```rust 137 | use bno055::{AxisRemap, BNO055AxisConfig}; 138 | // ... 139 | 140 | // Build remap configuration example with X and Y axes swapped: 141 | let remap = AxisRemap::builder() 142 | .swap_x_with(BNO055AxisConfig::AXIS_AS_Y) 143 | .build() 144 | .expect("Failed to build axis remap config"); 145 | 146 | bno055.set_axis_remap(remap)?; 147 | ``` 148 | 149 | Please note that `AxisRemap` builder (and the chip itself) doesn't allow an invalid state to be constructed, 150 | that is, when one axis is swapped with multiple of others. 151 | For example, swapping axis `X` with both `Y` and `Z` at the same time is not allowed: 152 | 153 | ```rust 154 | AxisRemap::builder() 155 | .swap_x_with(BNO055AxisConfig::AXIS_AS_Y) 156 | .swap_x_with(BNO055AxisConfig::AXIS_AS_Z) 157 | .build() 158 | .unwrap(); // <- panics, .build() returned Err 159 | ``` 160 | 161 | ### Changing axes sign 162 | 163 | It is also possible to flip the sign of either axis of the chip. 164 | 165 | Example of flipping X and Y axes: 166 | 167 | ```rust 168 | bno055 169 | .set_axis_sign(BNO055AxisSign::X_NEGATIVE | bno055::BNO055AxisSign::Y_NEGATIVE) 170 | .expect("Unable to communicate"); 171 | ``` 172 | 173 | ### Using external 32k crystal 174 | 175 | For better performance, it is recommended to connect and use external 32k quartz crystal. 176 | 177 | You enable or disable its use by calling `set_external_crystal`: 178 | 179 | ```rust 180 | bno055 181 | .set_external_crystal(true) 182 | .expect("Failed to set to external crystal"); 183 | ``` 184 | 185 | ### Using alternative I2C address 186 | 187 | BNO055 allows to change its I2C address from default `0x29` to alternative `0x28` by setting 188 | `COM3` pin `LOW`. 189 | 190 | To connect to device with an alternative address, enable its use by calling `with_alternative_address()`: 191 | 192 | ```rust 193 | // use default 0x29 address 194 | let mut bno = bno055::Bno055::new(i2c); 195 | 196 | // or: 197 | 198 | // use 0x28 address 199 | let mut bno = bno055::Bno055::new(i2c).with_alternative_address(); 200 | ``` 201 | 202 | ### Change BNO055 power mode 203 | 204 | ```rust 205 | use bno055::{Bno055, BNO055PowerMode}; 206 | // Normal mode 207 | bno055.set_power_mode(BNO055PowerMode::NORMAL)?; 208 | 209 | // Low-power mode (only accelerometer being awake) 210 | bno055.set_power_mode(BNO055PowerMode::LOW_POWER)?; 211 | 212 | // Suspend mode (all sensors and controller are sleeping) 213 | bno055.set_power_mode(BNO055PowerMode::SUSPEND)?; 214 | ``` 215 | 216 | ### Read chip temperature 217 | 218 | Temperature is specified in degrees Celsius by default. 219 | 220 | ```rust 221 | let temp: i8 = bno055.temperature()?; 222 | ``` 223 | 224 | ## Status 225 | 226 | What is done and tested and what is not yet: 227 | 228 | - [x] Sensor initialization 229 | - [x] Device mode setup 230 | - [x] Device status readout 231 | - [x] Calibration status readout 232 | - [x] External crystal selection 233 | - [x] Axis remap 234 | - [x] Axis sign setup 235 | - [x] Calibration data readout 236 | - [x] Calibration data setup 237 | - [x] Alternative I2C address 238 | - [x] Take register pages into account 239 | - [x] Orientation data readout 240 | - [x] Quaternions 241 | - [x] Euler angles 242 | - [x] Raw sensor data readout 243 | - [x] Raw accelerometer data readout 244 | - [x] Raw gyroscope data readout 245 | - [x] Raw magnetometer data readout 246 | - [x] Linear acceleration data readout 247 | - [x] Gravity vector data readout 248 | - [x] Temperature readout 249 | - [ ] Per-sensor configuration (when not in fusion mode) 250 | - [ ] Unit selection 251 | - [ ] Interrupts 252 | 253 | License: MIT. 254 | 255 | **Contributions welcomed!** 256 | -------------------------------------------------------------------------------- /bno055.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/eupn/bno055/e179c70161b918a96c9244ff453b099d37640de6/bno055.jpg -------------------------------------------------------------------------------- /examples/calibrate.rs: -------------------------------------------------------------------------------- 1 | use bno055::{BNO055OperationMode, Bno055}; 2 | use linux_embedded_hal::{Delay, I2cdev}; 3 | use mint::{EulerAngles, Quaternion}; 4 | 5 | fn main() { 6 | let dev = I2cdev::new("/dev/i2c-0").unwrap(); 7 | let mut delay = Delay {}; 8 | let mut imu = Bno055::new(dev).with_alternative_address(); 9 | imu.init(&mut delay) 10 | .expect("An error occurred while building the IMU"); 11 | 12 | imu.set_mode(BNO055OperationMode::NDOF, &mut delay) 13 | .expect("An error occurred while setting the IMU mode"); 14 | 15 | let mut status = imu.get_calibration_status().unwrap(); 16 | println!("The IMU's calibration status is: {:?}", status); 17 | 18 | // Wait for device to auto-calibrate. 19 | // Please perform steps necessary for auto-calibration to kick in. 20 | // Required steps are described in Datasheet section 3.11 21 | // Page 51, https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf (As of 2021-07-02) 22 | println!("- About to begin BNO055 IMU calibration..."); 23 | while !imu.is_fully_calibrated().unwrap() { 24 | status = imu.get_calibration_status().unwrap(); 25 | std::thread::sleep(std::time::Duration::from_millis(1000)); 26 | println!("Calibration status: {:?}", status); 27 | } 28 | 29 | let calib = imu.calibration_profile(&mut delay).unwrap(); 30 | 31 | imu.set_calibration_profile(calib, &mut delay).unwrap(); 32 | println!(" - Calibration complete!"); 33 | 34 | // These are sensor fusion reading using the mint crate that the state will be read into 35 | let mut euler_angles: EulerAngles; // = EulerAngles::::from([0.0, 0.0, 0.0]); 36 | let mut quaternion: Quaternion; // = Quaternion::::from([0.0, 0.0, 0.0, 0.0]); 37 | 38 | loop { 39 | // Quaternion; due to a bug in the BNO055, this is recommended over Euler Angles 40 | match imu.quaternion() { 41 | Ok(val) => { 42 | quaternion = val; 43 | println!("IMU Quaternion: {:?}", quaternion); 44 | std::thread::sleep(std::time::Duration::from_millis(500)); 45 | } 46 | Err(e) => { 47 | eprintln!("{:?}", e); 48 | } 49 | } 50 | 51 | // Euler angles, directly read 52 | match imu.euler_angles() { 53 | Ok(val) => { 54 | euler_angles = val; 55 | println!("IMU angles: {:?}", euler_angles); 56 | std::thread::sleep(std::time::Duration::from_millis(500)); 57 | } 58 | Err(e) => { 59 | eprintln!("{:?}", e); 60 | } 61 | } 62 | } 63 | } 64 | -------------------------------------------------------------------------------- /fuzz/.gitignore: -------------------------------------------------------------------------------- 1 | target 2 | corpus 3 | artifacts 4 | -------------------------------------------------------------------------------- /fuzz/Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "bno055-fuzz" 3 | version = "0.0.0" 4 | authors = ["Automatically generated"] 5 | publish = false 6 | edition = "2018" 7 | 8 | [package.metadata] 9 | cargo-fuzz = true 10 | 11 | [dependencies] 12 | libfuzzer-sys = "0.4" 13 | embedded-hal-fuzz = "1.0" 14 | embedded-hal = "1.0" 15 | 16 | [dependencies.bno055] 17 | path = ".." 18 | 19 | # Prevent this from interfering with workspaces 20 | [workspace] 21 | members = ["."] 22 | 23 | [[bin]] 24 | name = "init" 25 | path = "fuzz_targets/init.rs" 26 | test = false 27 | doc = false 28 | 29 | [[bin]] 30 | name = "calibrate" 31 | path = "fuzz_targets/calibrate.rs" 32 | test = false 33 | doc = false 34 | -------------------------------------------------------------------------------- /fuzz/fuzz_targets/calibrate.rs: -------------------------------------------------------------------------------- 1 | #![no_main] 2 | /// This is a modified version of the 'example' project optimized for fuzzing. 3 | 4 | use libfuzzer_sys::fuzz_target; 5 | 6 | use embedded_hal::{delay::DelayNs, i2c::SevenBitAddress}; 7 | use embedded_hal_fuzz::i2c::ArbitraryI2c; 8 | 9 | use bno055::{BNO055OperationMode, Bno055}; 10 | 11 | struct Delay {} 12 | 13 | impl Delay { pub fn new() -> Self { Delay{ } }} 14 | 15 | impl DelayNs for Delay { 16 | fn delay_ns(&mut self, _ns: u32) { 17 | // no-op, go as fast as possible for fuzzing 18 | } 19 | } 20 | 21 | 22 | fuzz_target!(|i2c: ArbitraryI2c| { 23 | let mut delay = Delay::new(); 24 | 25 | let mut imu = Bno055::new(i2c).with_alternative_address(); 26 | 27 | let _ = imu.init(&mut delay); 28 | 29 | let _ = imu.set_mode(BNO055OperationMode::NDOF, &mut delay); 30 | 31 | let _ = imu.get_calibration_status(); 32 | 33 | if !imu.is_fully_calibrated().is_ok() { 34 | let _ = imu.get_calibration_status(); 35 | } 36 | 37 | if let Ok(calib) = imu.calibration_profile(&mut delay) { 38 | let _ = imu.set_calibration_profile(calib, &mut delay); 39 | } 40 | 41 | // Quaternion; due to a bug in the BNO055, this is recommended over Euler Angles 42 | let _quaternion = imu.quaternion(); 43 | 44 | // Euler angles, directly read 45 | let _euler = imu.euler_angles(); 46 | 47 | // Temperature. 48 | let _temp = imu.temperature(); 49 | }); 50 | -------------------------------------------------------------------------------- /fuzz/fuzz_targets/init.rs: -------------------------------------------------------------------------------- 1 | #![no_main] 2 | use libfuzzer_sys::fuzz_target; 3 | 4 | use embedded_hal::{delay::DelayNs, i2c::SevenBitAddress}; 5 | use embedded_hal_fuzz::i2c::ArbitraryI2c; 6 | 7 | struct Delay {} 8 | 9 | impl Delay { 10 | pub fn new() -> Self { 11 | Delay {} 12 | } 13 | } 14 | 15 | impl DelayNs for Delay { 16 | fn delay_ns(&mut self, _ns: u32) { 17 | // no-op, go as fast as possible for fuzzing 18 | } 19 | } 20 | 21 | fuzz_target!(|i2c: ArbitraryI2c| { 22 | let mut delay = Delay::new(); 23 | 24 | // Init BNO055 IMU 25 | let mut imu = bno055::Bno055::new(i2c); 26 | 27 | // Discard the result as we only care about it it crashes not if there 28 | // is an error. 29 | let _ = imu.init(&mut delay); 30 | }); 31 | -------------------------------------------------------------------------------- /src/acc_config.rs: -------------------------------------------------------------------------------- 1 | use num_derive::FromPrimitive; 2 | use num_traits::FromPrimitive; 3 | 4 | #[allow(clippy::unusual_byte_groupings)] 5 | const ACC_G_RANGE_MASK: u8 = 0b000_000_11; 6 | #[allow(clippy::unusual_byte_groupings)] 7 | const ACC_BANDWIDTH_MASK: u8 = 0b000_111_00; 8 | #[allow(clippy::unusual_byte_groupings)] 9 | const ACC_OPERATION_MODE_MASK: u8 = 0b111_000_00; 10 | 11 | #[derive(Debug)] 12 | #[cfg_attr(feature = "defmt-03", derive(defmt::Format))] 13 | #[allow(clippy::enum_variant_names)] 14 | pub enum Error { 15 | BadAccGRange, 16 | BadAccBandwidth, 17 | BadAccOperationMode, 18 | } 19 | 20 | #[derive(Debug, Clone, Copy, FromPrimitive)] 21 | #[cfg_attr(feature = "defmt-03", derive(defmt::Format))] 22 | #[repr(u8)] 23 | #[allow(clippy::unusual_byte_groupings)] 24 | pub enum AccGRange { 25 | G2 = 0b000_000_00, 26 | G4 = 0b000_000_01, 27 | G8 = 0b000_000_10, 28 | G16 = 0b000_000_11, 29 | } 30 | 31 | #[derive(Debug, Clone, Copy, FromPrimitive)] 32 | #[cfg_attr(feature = "defmt-03", derive(defmt::Format))] 33 | #[repr(u8)] 34 | #[allow(clippy::unusual_byte_groupings)] 35 | pub enum AccBandwidth { 36 | Hz7_81 = 0b000_000_00, 37 | Hz15_63 = 0b000_001_00, 38 | Hz31_25 = 0b000_010_00, 39 | Hz62_5 = 0b000_011_00, 40 | Hz125 = 0b000_100_00, 41 | Hz250 = 0b000_101_00, 42 | Hz500 = 0b000_110_00, 43 | Hz1000 = 0b000_111_00, 44 | } 45 | 46 | #[derive(Debug, Clone, Copy, FromPrimitive)] 47 | #[cfg_attr(feature = "defmt-03", derive(defmt::Format))] 48 | #[repr(u8)] 49 | #[allow(clippy::unusual_byte_groupings)] 50 | pub enum AccOperationMode { 51 | Normal = 0b000_000_00, 52 | Suspend = 0b001_000_00, 53 | LowPower1 = 0b010_000_00, 54 | Standby = 0b011_000_00, 55 | LowPower2 = 0b100_000_00, 56 | DeepSuspend = 0b101_000_00, 57 | } 58 | 59 | #[derive(Debug, Clone)] 60 | #[cfg_attr(feature = "defmt-03", derive(defmt::Format))] 61 | pub struct AccConfig { 62 | g_range: AccGRange, 63 | bandwidth: AccBandwidth, 64 | operation_mode: AccOperationMode, 65 | } 66 | 67 | impl AccConfig { 68 | pub fn try_from_bits(bits: u8) -> Result { 69 | let g_range = AccGRange::from_u8(bits & ACC_G_RANGE_MASK).ok_or(Error::BadAccGRange)?; 70 | let bandwidth = 71 | AccBandwidth::from_u8(bits & ACC_BANDWIDTH_MASK).ok_or(Error::BadAccBandwidth)?; 72 | let operation_mode = AccOperationMode::from_u8(bits & ACC_OPERATION_MODE_MASK) 73 | .ok_or(Error::BadAccOperationMode)?; 74 | 75 | Ok(Self { 76 | g_range, 77 | bandwidth, 78 | operation_mode, 79 | }) 80 | } 81 | 82 | pub fn bits(&self) -> u8 { 83 | self.operation_mode as u8 | self.bandwidth as u8 | self.g_range as u8 84 | } 85 | 86 | pub fn g_range(&self) -> AccGRange { 87 | self.g_range 88 | } 89 | 90 | pub fn bandwidth(&self) -> AccBandwidth { 91 | self.bandwidth 92 | } 93 | 94 | pub fn operation_mode(&self) -> AccOperationMode { 95 | self.operation_mode 96 | } 97 | 98 | pub fn set_g_range(&mut self, g_range: AccGRange) { 99 | self.g_range = g_range; 100 | } 101 | 102 | pub fn set_bandwidth(&mut self, bandwidth: AccBandwidth) { 103 | self.bandwidth = bandwidth; 104 | } 105 | 106 | pub fn set_operation_mode(&mut self, operation_mode: AccOperationMode) { 107 | self.operation_mode = operation_mode; 108 | } 109 | } 110 | -------------------------------------------------------------------------------- /src/lib.rs: -------------------------------------------------------------------------------- 1 | #![doc(html_root_url = "https://docs.rs/bno055/0.4.0")] 2 | #![cfg_attr(not(feature = "std"), no_std)] 3 | #![allow(clippy::bad_bit_mask)] 4 | 5 | //! Bosch Sensortec BNO055 9-axis IMU sensor driver. 6 | //! Datasheet: https://ae-bst.resource.bosch.com/media/_tech/media/datasheets/BST-BNO055-DS000.pdf 7 | use embedded_hal::{ 8 | delay::DelayNs, 9 | i2c::{I2c, SevenBitAddress}, 10 | }; 11 | 12 | #[cfg(not(feature = "defmt-03"))] 13 | use bitflags::bitflags; 14 | #[cfg(feature = "defmt-03")] 15 | use defmt::bitflags; 16 | 17 | use byteorder::{ByteOrder, LittleEndian}; 18 | pub use mint; 19 | #[cfg(feature = "serde")] 20 | use serde::{Deserialize, Serialize}; 21 | 22 | mod acc_config; 23 | mod regs; 24 | #[cfg(feature = "std")] 25 | mod std; 26 | 27 | pub use acc_config::{AccBandwidth, AccConfig, AccGRange, AccOperationMode}; 28 | pub use regs::BNO055_ID; 29 | 30 | /// All possible errors in this crate 31 | #[derive(Debug)] 32 | #[cfg_attr(feature = "defmt-03", derive(defmt::Format))] 33 | pub enum Error { 34 | /// I2C bus error 35 | I2c(E), 36 | 37 | /// Invalid chip ID was read 38 | InvalidChipId(u8), 39 | 40 | /// Invalid (not applicable) device mode. 41 | InvalidMode, 42 | 43 | /// Accelerometer configuration error 44 | AccConfig(acc_config::Error), 45 | } 46 | 47 | #[cfg_attr(feature = "defmt-03", derive(defmt::Format))] 48 | pub struct Bno055 { 49 | i2c: I, 50 | pub mode: BNO055OperationMode, 51 | use_default_addr: bool, 52 | } 53 | 54 | impl Bno055 55 | where 56 | I: I2c, 57 | { 58 | /// Side-effect-free constructor. 59 | /// Nothing will be read or written before `init()` call. 60 | pub fn new(i2c: I) -> Self { 61 | Bno055 { 62 | i2c, 63 | mode: BNO055OperationMode::CONFIG_MODE, 64 | use_default_addr: true, 65 | } 66 | } 67 | 68 | /// Destroy driver instance, return I2C bus instance. 69 | pub fn destroy(self) -> I { 70 | self.i2c 71 | } 72 | 73 | /// Enables use of alternative I2C address `regs::BNO055_ALTERNATE_ADDR`. 74 | pub fn with_alternative_address(mut self) -> Self { 75 | self.use_default_addr = false; 76 | 77 | self 78 | } 79 | 80 | /// Initializes the BNO055 device. 81 | /// 82 | /// Side-effects: 83 | /// - Software reset of BNO055 84 | /// - Sets BNO055 to `CONFIG` mode 85 | /// - Sets BNO055's power mode to `NORMAL` 86 | /// - Clears `SYS_TRIGGER` register 87 | /// 88 | /// # Usage Example 89 | /// 90 | /// ```rust 91 | /// // use your_chip_hal::{I2c, Delay}; // <- import your chip's I2c and Delay 92 | /// use bno055::Bno055; 93 | /// # 94 | /// # // All of this is needed for example to work: 95 | /// # use bno055::BNO055_ID; 96 | /// # use embedded_hal::delay::DelayNs; 97 | /// # use embedded_hal::i2c::{I2c as I2cTrait, Operation, Error, ErrorType, ErrorKind}; 98 | /// # struct Delay {} 99 | /// # impl Delay { pub fn new() -> Self { Delay{ } }} 100 | /// # impl DelayNs for Delay { 101 | /// # fn delay_ns(&mut self, ms: u32) { 102 | /// # // no-op for example purposes 103 | /// # } 104 | /// # } 105 | /// # struct I2c {} 106 | /// # impl I2c { pub fn new() -> Self { I2c { } }} 107 | /// # #[derive(Debug)] 108 | /// # struct DummyError {} 109 | /// # impl Error for DummyError { fn kind(&self) -> ErrorKind { ErrorKind::Other } } 110 | /// # impl ErrorType for I2c { type Error = DummyError; } 111 | /// # // 3 calls are made, 2 Writes and 1 Write/Read. We want to mock the 3rd call's read. 112 | /// # impl I2cTrait for I2c { fn transaction(&mut self, address: u8, operations: &mut [Operation<'_>]) -> Result<(), Self::Error> { match operations.get_mut(1) { Some(Operation::Read(read)) => { read[0] = BNO055_ID; }, _ => {} }; Ok(()) } } 113 | /// # 114 | /// # // Actual example: 115 | /// let mut delay = Delay::new(/* ... */); 116 | /// let mut i2c = I2c::new(/* ... */); 117 | /// let mut bno055 = Bno055::new(i2c); 118 | /// bno055.init(&mut delay)?; 119 | /// # Result::<(), bno055::Error>::Ok(()) 120 | /// ``` 121 | pub fn init(&mut self, delay: &mut dyn DelayNs) -> Result<(), Error> { 122 | self.set_page(BNO055RegisterPage::PAGE_0)?; 123 | 124 | let id = self.id()?; 125 | if id != regs::BNO055_ID { 126 | return Err(Error::InvalidChipId(id)); 127 | } 128 | 129 | self.soft_reset(delay)?; 130 | self.set_mode(BNO055OperationMode::CONFIG_MODE, delay)?; 131 | self.set_power_mode(BNO055PowerMode::NORMAL)?; 132 | self.write_u8(regs::BNO055_SYS_TRIGGER, 0x00) 133 | .map_err(Error::I2c)?; 134 | 135 | Ok(()) 136 | } 137 | 138 | /// Resets the BNO055, initializing the register map to default values. 139 | /// More in section 3.2. 140 | pub fn soft_reset(&mut self, delay: &mut dyn DelayNs) -> Result<(), Error> { 141 | self.set_page(BNO055RegisterPage::PAGE_0)?; 142 | 143 | self.write_u8( 144 | regs::BNO055_SYS_TRIGGER, 145 | regs::BNO055_SYS_TRIGGER_RST_SYS_BIT, 146 | ) 147 | .map_err(Error::I2c)?; 148 | 149 | // As per table 1.2 150 | delay.delay_ms(650); 151 | Ok(()) 152 | } 153 | 154 | /// Sets the operating mode, see [BNO055OperationMode](enum.BNO055OperationMode.html). 155 | /// See section 3.3. 156 | pub fn set_mode( 157 | &mut self, 158 | mode: BNO055OperationMode, 159 | delay: &mut dyn DelayNs, 160 | ) -> Result<(), Error> { 161 | if self.mode != mode { 162 | self.set_page(BNO055RegisterPage::PAGE_0)?; 163 | 164 | self.mode = mode; 165 | 166 | self.write_u8(regs::BNO055_OPR_MODE, mode.bits()) 167 | .map_err(Error::I2c)?; 168 | 169 | // Table 3-6 says 19ms to switch to CONFIG_MODE 170 | delay.delay_ms(19); 171 | } 172 | 173 | Ok(()) 174 | } 175 | 176 | /// Sets the power mode, see [BNO055PowerMode](enum.BNO055PowerMode.html) 177 | /// See section 3.2 178 | pub fn set_power_mode(&mut self, mode: BNO055PowerMode) -> Result<(), Error> { 179 | self.set_page(BNO055RegisterPage::PAGE_0)?; 180 | 181 | self.write_u8(regs::BNO055_PWR_MODE, mode.bits()) 182 | .map_err(Error::I2c)?; 183 | 184 | Ok(()) 185 | } 186 | 187 | /// Returns BNO055's power mode. 188 | pub fn power_mode(&mut self) -> Result> { 189 | self.set_page(BNO055RegisterPage::PAGE_0)?; 190 | 191 | let mode = self.read_u8(regs::BNO055_PWR_MODE).map_err(Error::I2c)?; 192 | 193 | Ok(BNO055PowerMode::from_bits_truncate(mode)) 194 | } 195 | 196 | /// Enables/Disables usage of external 32k crystal. 197 | pub fn set_external_crystal( 198 | &mut self, 199 | ext: bool, 200 | delay: &mut dyn DelayNs, 201 | ) -> Result<(), Error> { 202 | self.set_page(BNO055RegisterPage::PAGE_0)?; 203 | 204 | let prev = self.mode; 205 | self.set_mode(BNO055OperationMode::CONFIG_MODE, delay)?; 206 | self.write_u8(regs::BNO055_SYS_TRIGGER, if ext { 0x80 } else { 0x00 }) 207 | .map_err(Error::I2c)?; 208 | 209 | self.set_mode(prev, delay)?; 210 | 211 | Ok(()) 212 | } 213 | 214 | /// Configures axis remap of the device. 215 | pub fn set_axis_remap(&mut self, remap: AxisRemap) -> Result<(), Error> { 216 | self.set_page(BNO055RegisterPage::PAGE_0)?; 217 | 218 | let remap_value = (remap.x.bits() & 0b11) 219 | | ((remap.y.bits() & 0b11) << 2) 220 | | ((remap.z.bits() & 0b11) << 4); 221 | 222 | self.write_u8(regs::BNO055_AXIS_MAP_CONFIG, remap_value) 223 | .map_err(Error::I2c)?; 224 | 225 | Ok(()) 226 | } 227 | 228 | /// Returns axis remap of the device. 229 | pub fn axis_remap(&mut self) -> Result> { 230 | self.set_page(BNO055RegisterPage::PAGE_0)?; 231 | 232 | let value = self 233 | .read_u8(regs::BNO055_AXIS_MAP_CONFIG) 234 | .map_err(Error::I2c)?; 235 | 236 | let remap = AxisRemap { 237 | x: BNO055AxisConfig::from_bits_truncate(value & 0b11), 238 | y: BNO055AxisConfig::from_bits_truncate((value >> 2) & 0b11), 239 | z: BNO055AxisConfig::from_bits_truncate((value >> 4) & 0b11), 240 | }; 241 | 242 | Ok(remap) 243 | } 244 | 245 | /// Configures device's axes sign: positive or negative. 246 | pub fn set_axis_sign(&mut self, sign: BNO055AxisSign) -> Result<(), Error> { 247 | self.set_page(BNO055RegisterPage::PAGE_0)?; 248 | 249 | self.write_u8(regs::BNO055_AXIS_MAP_SIGN, sign.bits()) 250 | .map_err(Error::I2c)?; 251 | 252 | Ok(()) 253 | } 254 | 255 | /// Return device's axes sign. 256 | pub fn axis_sign(&mut self) -> Result> { 257 | self.set_page(BNO055RegisterPage::PAGE_0)?; 258 | 259 | let value = self 260 | .read_u8(regs::BNO055_AXIS_MAP_SIGN) 261 | .map_err(Error::I2c)?; 262 | 263 | Ok(BNO055AxisSign::from_bits_truncate(value)) 264 | } 265 | 266 | /// Gets the revision of software, bootloader, accelerometer, magnetometer, and gyroscope of 267 | /// the BNO055 device. 268 | pub fn get_revision(&mut self) -> Result> { 269 | self.set_page(BNO055RegisterPage::PAGE_0)?; 270 | 271 | let mut buf: [u8; 6] = [0; 6]; 272 | 273 | self.read_bytes(regs::BNO055_ACC_ID, &mut buf) 274 | .map_err(Error::I2c)?; 275 | 276 | Ok(BNO055Revision { 277 | software: LittleEndian::read_u16(&buf[3..5]), 278 | bootloader: buf[5], 279 | accelerometer: buf[0], 280 | magnetometer: buf[1], 281 | gyroscope: buf[2], 282 | }) 283 | } 284 | 285 | /// Returns device's system status. 286 | pub fn get_system_status( 287 | &mut self, 288 | do_selftest: bool, 289 | delay: &mut dyn DelayNs, 290 | ) -> Result> { 291 | self.set_page(BNO055RegisterPage::PAGE_0)?; 292 | 293 | let selftest = if do_selftest { 294 | let prev = self.mode; 295 | self.set_mode(BNO055OperationMode::CONFIG_MODE, delay)?; 296 | 297 | let sys_trigger = self.read_u8(regs::BNO055_SYS_TRIGGER).map_err(Error::I2c)?; 298 | 299 | self.write_u8(regs::BNO055_SYS_TRIGGER, sys_trigger | 0x1) 300 | .map_err(Error::I2c)?; 301 | 302 | // Wait for self-test result 303 | for _ in 0..4 { 304 | delay.delay_ms(255); 305 | } 306 | 307 | let result = self.read_u8(regs::BNO055_ST_RESULT).map_err(Error::I2c)?; 308 | 309 | self.set_mode(prev, delay)?; // Restore previous mode 310 | 311 | Some(BNO055SelfTestStatus::from_bits_truncate(result)) 312 | } else { 313 | None 314 | }; 315 | 316 | let status = self.read_u8(regs::BNO055_SYS_STATUS).map_err(Error::I2c)?; 317 | let error = self.read_u8(regs::BNO055_SYS_ERR).map_err(Error::I2c)?; 318 | 319 | Ok(BNO055SystemStatus { 320 | status: BNO055SystemStatusCode::from_bits_truncate(status), 321 | error: BNO055SystemErrorCode::from_bits_truncate(error), 322 | selftest, 323 | }) 324 | } 325 | 326 | /// Gets a quaternion (`mint::Quaternion`) reading from the BNO055. 327 | /// Available only in sensor fusion modes. 328 | pub fn quaternion(&mut self) -> Result, Error> { 329 | self.set_page(BNO055RegisterPage::PAGE_0)?; 330 | 331 | // Device should be in fusion mode to be able to produce quaternions 332 | if self.mode.is_fusion_enabled() { 333 | let mut buf: [u8; 8] = [0; 8]; 334 | self.read_bytes(regs::BNO055_QUA_DATA_W_LSB, &mut buf) 335 | .map_err(Error::I2c)?; 336 | 337 | let w = LittleEndian::read_i16(&buf[0..2]); 338 | let x = LittleEndian::read_i16(&buf[2..4]); 339 | let y = LittleEndian::read_i16(&buf[4..6]); 340 | let z = LittleEndian::read_i16(&buf[6..8]); 341 | 342 | let scale = 1.0 / ((1 << 14) as f32); 343 | 344 | let x = x as f32 * scale; 345 | let y = y as f32 * scale; 346 | let z = z as f32 * scale; 347 | let w = w as f32 * scale; 348 | 349 | let quat = mint::Quaternion { 350 | v: mint::Vector3 { x, y, z }, 351 | s: w, 352 | }; 353 | 354 | Ok(quat) 355 | } else { 356 | Err(Error::InvalidMode) 357 | } 358 | } 359 | 360 | /// Get Euler angles representation of heading in degrees. 361 | /// Euler angles is represented as (`roll`, `pitch`, `yaw/heading`). 362 | /// Available only in sensor fusion modes. 363 | pub fn euler_angles(&mut self) -> Result, Error> { 364 | self.set_page(BNO055RegisterPage::PAGE_0)?; 365 | 366 | // Device should be in fusion mode to be able to produce Euler angles 367 | if self.mode.is_fusion_enabled() { 368 | let mut buf: [u8; 6] = [0; 6]; 369 | 370 | self.read_bytes(regs::BNO055_EUL_HEADING_LSB, &mut buf) 371 | .map_err(Error::I2c)?; 372 | 373 | let heading = LittleEndian::read_i16(&buf[0..2]) as f32; 374 | let roll = LittleEndian::read_i16(&buf[2..4]) as f32; 375 | let pitch = LittleEndian::read_i16(&buf[4..6]) as f32; 376 | 377 | let scale = 1f32 / 16f32; // 1 degree = 16 LSB 378 | 379 | let rot = mint::EulerAngles::from([roll * scale, pitch * scale, heading * scale]); 380 | 381 | Ok(rot) 382 | } else { 383 | Err(Error::InvalidMode) 384 | } 385 | } 386 | 387 | /// Get calibration status 388 | pub fn get_calibration_status(&mut self) -> Result> { 389 | self.set_page(BNO055RegisterPage::PAGE_0)?; 390 | 391 | let status = self.read_u8(regs::BNO055_CALIB_STAT).map_err(Error::I2c)?; 392 | 393 | let sys = (status >> 6) & 0b11; 394 | let gyr = (status >> 4) & 0b11; 395 | let acc = (status >> 2) & 0b11; 396 | let mag = status & 0b11; 397 | 398 | Ok(BNO055CalibrationStatus { sys, gyr, acc, mag }) 399 | } 400 | 401 | /// Checks whether device is fully calibrated or not. 402 | pub fn is_fully_calibrated(&mut self) -> Result> { 403 | let status = self.get_calibration_status()?; 404 | Ok(status.mag == 3 && status.gyr == 3 && status.acc == 3 && status.sys == 3) 405 | } 406 | 407 | /// Reads current calibration profile of the device. 408 | pub fn calibration_profile( 409 | &mut self, 410 | delay: &mut dyn DelayNs, 411 | ) -> Result> { 412 | self.set_page(BNO055RegisterPage::PAGE_0)?; 413 | 414 | let prev_mode = self.mode; 415 | self.set_mode(BNO055OperationMode::CONFIG_MODE, delay)?; 416 | 417 | let mut buf: [u8; BNO055_CALIB_SIZE] = [0; BNO055_CALIB_SIZE]; 418 | 419 | self.read_bytes(regs::BNO055_ACC_OFFSET_X_LSB, &mut buf[..]) 420 | .map_err(Error::I2c)?; 421 | 422 | let res = BNO055Calibration::from_buf(&buf); 423 | 424 | self.set_mode(prev_mode, delay)?; 425 | 426 | Ok(res) 427 | } 428 | 429 | /// Sets current calibration profile. 430 | pub fn set_calibration_profile( 431 | &mut self, 432 | calib: BNO055Calibration, 433 | delay: &mut dyn DelayNs, 434 | ) -> Result<(), Error> { 435 | self.set_page(BNO055RegisterPage::PAGE_0)?; 436 | 437 | let prev_mode = self.mode; 438 | self.set_mode(BNO055OperationMode::CONFIG_MODE, delay)?; 439 | 440 | let buf_profile = calib.as_bytes(); 441 | 442 | // Combine register address and profile into single buffer 443 | let buf_reg = [regs::BNO055_ACC_OFFSET_X_LSB; 1]; 444 | let mut buf_with_reg = [0u8; 1 + BNO055_CALIB_SIZE]; 445 | for (to, from) in buf_with_reg 446 | .iter_mut() 447 | .zip(buf_reg.iter().chain(buf_profile.iter())) 448 | { 449 | *to = *from 450 | } 451 | 452 | self.i2c 453 | .write(self.i2c_addr(), &buf_with_reg[..]) 454 | .map_err(Error::I2c)?; 455 | 456 | self.set_mode(prev_mode, delay)?; 457 | 458 | Ok(()) 459 | } 460 | 461 | /// Returns device's factory-programmed and constant chip ID. 462 | /// This ID is device model ID and not a BNO055's unique ID, whic is stored in different register. 463 | pub fn id(&mut self) -> Result> { 464 | self.set_page(BNO055RegisterPage::PAGE_0)?; 465 | self.read_u8(regs::BNO055_CHIP_ID).map_err(Error::I2c) 466 | } 467 | 468 | /// Returns device's operation mode. 469 | pub fn get_mode(&mut self) -> Result> { 470 | self.set_page(BNO055RegisterPage::PAGE_0)?; 471 | 472 | let mode = self.read_u8(regs::BNO055_OPR_MODE).map_err(Error::I2c)?; 473 | let mode = BNO055OperationMode::from_bits_truncate(mode); 474 | self.mode = mode; 475 | 476 | Ok(mode) 477 | } 478 | 479 | /// Checks whether the device is in Sensor Fusion mode or not. 480 | pub fn is_in_fusion_mode(&mut self) -> Result> { 481 | Ok(self.mode.is_fusion_enabled()) 482 | } 483 | 484 | pub fn get_acc_config(&mut self) -> Result> { 485 | self.set_page(BNO055RegisterPage::PAGE_1)?; 486 | 487 | let bits = self.read_u8(regs::BNO055_ACC_CONFIG).map_err(Error::I2c)?; 488 | 489 | let acc_config = AccConfig::try_from_bits(bits).map_err(Error::AccConfig)?; 490 | 491 | Ok(acc_config) 492 | } 493 | 494 | pub fn set_acc_config(&mut self, acc_config: &AccConfig) -> Result<(), Error> { 495 | self.set_page(BNO055RegisterPage::PAGE_1)?; 496 | 497 | self.write_u8(regs::BNO055_ACC_CONFIG, acc_config.bits()) 498 | .map_err(Error::I2c)?; 499 | 500 | Ok(()) 501 | } 502 | 503 | /// Sets current register map page. 504 | fn set_page(&mut self, page: BNO055RegisterPage) -> Result<(), Error> { 505 | self.write_u8(regs::BNO055_PAGE_ID, page.bits()) 506 | .map_err(Error::I2c)?; 507 | 508 | Ok(()) 509 | } 510 | 511 | /// Reads a vector of sensor data from the device. 512 | fn read_vec_raw(&mut self, reg: u8) -> Result, Error> { 513 | let mut buf: [u8; 6] = [0; 6]; 514 | 515 | self.read_bytes(reg, &mut buf).map_err(Error::I2c)?; 516 | 517 | let x = LittleEndian::read_i16(&buf[0..2]); 518 | let y = LittleEndian::read_i16(&buf[2..4]); 519 | let z = LittleEndian::read_i16(&buf[4..6]); 520 | 521 | Ok(mint::Vector3::from([x, y, z])) 522 | } 523 | 524 | /// Applies the given scaling to the vector of sensor data from the device. 525 | fn scale_vec(raw: mint::Vector3, scaling: f32) -> mint::Vector3 { 526 | mint::Vector3::from([ 527 | raw.x as f32 * scaling, 528 | raw.y as f32 * scaling, 529 | raw.z as f32 * scaling, 530 | ]) 531 | } 532 | 533 | /// Returns linear acceleration vector in cm/s^2 units. 534 | /// Available only in sensor fusion modes. 535 | pub fn linear_acceleration_fixed(&mut self) -> Result, Error> { 536 | if self.mode.is_fusion_enabled() { 537 | self.set_page(BNO055RegisterPage::PAGE_0)?; 538 | self.read_vec_raw(regs::BNO055_LIA_DATA_X_LSB) 539 | } else { 540 | Err(Error::InvalidMode) 541 | } 542 | } 543 | 544 | /// Returns linear acceleration vector in m/s^2 units. 545 | /// Available only in sensor fusion modes. 546 | pub fn linear_acceleration(&mut self) -> Result, Error> { 547 | let linear_acceleration = self.linear_acceleration_fixed()?; 548 | let scaling = 1f32 / 100f32; // 1 m/s^2 = 100 lsb 549 | Ok(Self::scale_vec(linear_acceleration, scaling)) 550 | } 551 | 552 | /// Returns gravity vector in cm/s^2 units. 553 | /// Available only in sensor fusion modes. 554 | pub fn gravity_fixed(&mut self) -> Result, Error> { 555 | if self.mode.is_fusion_enabled() { 556 | self.set_page(BNO055RegisterPage::PAGE_0)?; 557 | self.read_vec_raw(regs::BNO055_GRV_DATA_X_LSB) 558 | } else { 559 | Err(Error::InvalidMode) 560 | } 561 | } 562 | 563 | /// Returns gravity vector in m/s^2 units. 564 | /// Available only in sensor fusion modes. 565 | pub fn gravity(&mut self) -> Result, Error> { 566 | let gravity = self.gravity_fixed()?; 567 | let scaling = 1f32 / 100f32; // 1 m/s^2 = 100 lsb 568 | Ok(Self::scale_vec(gravity, scaling)) 569 | } 570 | 571 | /// Returns current accelerometer data in cm/s^2 units. 572 | /// Available only in modes in which accelerometer is enabled. 573 | pub fn accel_data_fixed(&mut self) -> Result, Error> { 574 | if self.mode.is_accel_enabled() { 575 | self.set_page(BNO055RegisterPage::PAGE_0)?; 576 | self.read_vec_raw(regs::BNO055_ACC_DATA_X_LSB) 577 | } else { 578 | Err(Error::InvalidMode) 579 | } 580 | } 581 | 582 | /// Returns current accelerometer data in m/s^2 units. 583 | /// Available only in modes in which accelerometer is enabled. 584 | pub fn accel_data(&mut self) -> Result, Error> { 585 | let a = self.accel_data_fixed()?; 586 | let scaling = 1f32 / 100f32; // 1 m/s^2 = 100 lsb 587 | Ok(Self::scale_vec(a, scaling)) 588 | } 589 | 590 | /// Returns current gyroscope data in 1/16th deg/s units. 591 | /// Available only in modes in which gyroscope is enabled. 592 | pub fn gyro_data_fixed(&mut self) -> Result, Error> { 593 | if self.mode.is_gyro_enabled() { 594 | self.set_page(BNO055RegisterPage::PAGE_0)?; 595 | self.read_vec_raw(regs::BNO055_GYR_DATA_X_LSB) 596 | } else { 597 | Err(Error::InvalidMode) 598 | } 599 | } 600 | 601 | /// Returns current gyroscope data in deg/s units. 602 | /// Available only in modes in which gyroscope is enabled. 603 | pub fn gyro_data(&mut self) -> Result, Error> { 604 | let g = self.gyro_data_fixed()?; 605 | let scaling = 1f32 / 16f32; // 1 deg/s = 16 lsb 606 | Ok(Self::scale_vec(g, scaling)) 607 | } 608 | 609 | /// Returns current magnetometer data in 1/16th uT units. 610 | /// Available only in modes in which magnetometer is enabled. 611 | pub fn mag_data_fixed(&mut self) -> Result, Error> { 612 | if self.mode.is_mag_enabled() { 613 | self.set_page(BNO055RegisterPage::PAGE_0)?; 614 | self.read_vec_raw(regs::BNO055_MAG_DATA_X_LSB) 615 | } else { 616 | Err(Error::InvalidMode) 617 | } 618 | } 619 | 620 | /// Returns current magnetometer data in uT units. 621 | /// Available only in modes in which magnetometer is enabled. 622 | pub fn mag_data(&mut self) -> Result, Error> { 623 | let m = self.mag_data_fixed()?; 624 | let scaling = 1f32 / 16f32; // 1 uT = 16 lsb 625 | Ok(Self::scale_vec(m, scaling)) 626 | } 627 | 628 | /// Returns current temperature of the chip (in degrees Celsius). 629 | pub fn temperature(&mut self) -> Result> { 630 | self.set_page(BNO055RegisterPage::PAGE_0)?; 631 | 632 | // Read temperature signed byte 633 | let temp = self.read_u8(regs::BNO055_TEMP).map_err(Error::I2c)? as i8; 634 | Ok(temp) 635 | } 636 | 637 | #[inline(always)] 638 | fn i2c_addr(&self) -> u8 { 639 | if !self.use_default_addr { 640 | regs::BNO055_ALTERNATE_ADDR 641 | } else { 642 | regs::BNO055_DEFAULT_ADDR 643 | } 644 | } 645 | 646 | fn read_u8(&mut self, reg: u8) -> Result { 647 | let mut byte: [u8; 1] = [0; 1]; 648 | 649 | match self.i2c.write_read(self.i2c_addr(), &[reg], &mut byte) { 650 | Ok(_) => Ok(byte[0]), 651 | Err(e) => Err(e), 652 | } 653 | } 654 | 655 | fn read_bytes(&mut self, reg: u8, buf: &mut [u8]) -> Result<(), E> { 656 | self.i2c.write_read(self.i2c_addr(), &[reg], buf) 657 | } 658 | 659 | fn write_u8(&mut self, reg: u8, value: u8) -> Result<(), E> { 660 | self.i2c.write(self.i2c_addr(), &[reg, value])?; 661 | 662 | Ok(()) 663 | } 664 | } 665 | 666 | bitflags! { 667 | #[cfg_attr(not(feature = "defmt-03"), derive(Debug, Clone, Copy, PartialEq, Eq))] 668 | pub struct BNO055AxisConfig: u8 { 669 | const AXIS_AS_X = 0b00; 670 | const AXIS_AS_Y = 0b01; 671 | const AXIS_AS_Z = 0b10; 672 | } 673 | } 674 | 675 | #[allow(clippy::misnamed_getters)] 676 | impl AxisRemap { 677 | pub fn x(&self) -> BNO055AxisConfig { 678 | self.x 679 | } 680 | 681 | pub fn y(&self) -> BNO055AxisConfig { 682 | self.x 683 | } 684 | 685 | pub fn z(&self) -> BNO055AxisConfig { 686 | self.z 687 | } 688 | } 689 | 690 | #[derive(Debug, Clone, Copy, PartialEq, Eq)] 691 | pub struct AxisRemap { 692 | x: BNO055AxisConfig, 693 | y: BNO055AxisConfig, 694 | z: BNO055AxisConfig, 695 | } 696 | 697 | #[derive(Debug)] 698 | pub struct AxisRemapBuilder { 699 | remap: AxisRemap, 700 | } 701 | 702 | impl AxisRemap { 703 | pub fn builder() -> AxisRemapBuilder { 704 | AxisRemapBuilder { 705 | remap: AxisRemap { 706 | x: BNO055AxisConfig::AXIS_AS_X, 707 | y: BNO055AxisConfig::AXIS_AS_Y, 708 | z: BNO055AxisConfig::AXIS_AS_Z, 709 | }, 710 | } 711 | } 712 | } 713 | 714 | impl AxisRemapBuilder { 715 | pub fn swap_x_with(mut self, to: BNO055AxisConfig) -> AxisRemapBuilder { 716 | let old_x = self.remap.x; 717 | 718 | match to { 719 | BNO055AxisConfig::AXIS_AS_X => self.remap.x = old_x, 720 | BNO055AxisConfig::AXIS_AS_Y => self.remap.y = old_x, 721 | BNO055AxisConfig::AXIS_AS_Z => self.remap.z = old_x, 722 | 723 | _ => (), 724 | } 725 | 726 | self.remap.x = to; 727 | 728 | AxisRemapBuilder { remap: self.remap } 729 | } 730 | 731 | pub fn swap_y_with(mut self, to: BNO055AxisConfig) -> AxisRemapBuilder { 732 | let old_y = self.remap.y; 733 | 734 | match to { 735 | BNO055AxisConfig::AXIS_AS_X => self.remap.x = old_y, 736 | BNO055AxisConfig::AXIS_AS_Y => self.remap.y = old_y, 737 | BNO055AxisConfig::AXIS_AS_Z => self.remap.z = old_y, 738 | 739 | _ => (), 740 | } 741 | 742 | self.remap.y = to; 743 | 744 | AxisRemapBuilder { remap: self.remap } 745 | } 746 | 747 | pub fn swap_z_with(mut self, to: BNO055AxisConfig) -> AxisRemapBuilder { 748 | let old_z = self.remap.z; 749 | 750 | match to { 751 | BNO055AxisConfig::AXIS_AS_X => self.remap.x = old_z, 752 | BNO055AxisConfig::AXIS_AS_Y => self.remap.y = old_z, 753 | BNO055AxisConfig::AXIS_AS_Z => self.remap.z = old_z, 754 | 755 | _ => (), 756 | } 757 | 758 | self.remap.z = to; 759 | 760 | AxisRemapBuilder { remap: self.remap } 761 | } 762 | 763 | fn is_invalid(&self) -> bool { 764 | // Each axis must be swapped only once, 765 | // For example, one cannot remap X to Y and Z to Y at the same time, or similar. 766 | // See datasheet, section 3.4. 767 | self.remap.x == self.remap.y || self.remap.y == self.remap.z || self.remap.z == self.remap.x 768 | } 769 | 770 | #[allow(clippy::result_unit_err)] 771 | pub fn build(self) -> Result { 772 | if self.is_invalid() { 773 | Err(()) 774 | } else { 775 | Ok(self.remap) 776 | } 777 | } 778 | } 779 | 780 | bitflags! { 781 | #[cfg_attr(not(feature = "defmt-03"), derive(Debug, Clone, Copy, PartialEq, Eq))] 782 | pub struct BNO055AxisSign: u8 { 783 | const X_NEGATIVE = 0b100; 784 | const Y_NEGATIVE = 0b010; 785 | const Z_NEGATIVE = 0b001; 786 | } 787 | } 788 | 789 | bitflags! { 790 | #[cfg_attr(not(feature = "defmt-03"), derive(Debug, Clone, Copy, PartialEq, Eq))] 791 | pub struct BNO055SystemStatusCode: u8 { 792 | const SYSTEM_IDLE = 0; 793 | const SYSTEM_ERROR = 1; 794 | const INIT_PERIPHERALS = 2; 795 | const SYSTEM_INIT = 3; 796 | const EXECUTING = 4; 797 | const RUNNING = 5; 798 | const RUNNING_WITHOUT_FUSION = 6; 799 | } 800 | } 801 | 802 | bitflags! { 803 | /// Possible BNO055 errors. 804 | #[cfg_attr(not(feature = "defmt-03"), derive(Debug, Clone, Copy, PartialEq, Eq))] 805 | pub struct BNO055SystemErrorCode: u8 { 806 | const NONE = 0; 807 | const PERIPHERAL_INIT = 1; 808 | const SYSTEM_INIT = 2; 809 | const SELF_TEST = 3; 810 | const REGISTER_MAP_VALUE = 4; 811 | const REGISTER_MAP_ADDRESS = 5; 812 | const REGISTER_MAP_WRITE = 6; 813 | const LOW_POWER_MODE_NOT_AVAIL = 7; 814 | const ACCEL_POWER_MODE_NOT_AVAIL = 8; 815 | const FUSION_ALGO_CONFIG = 9; 816 | const SENSOR_CONFIG = 10; 817 | } 818 | } 819 | 820 | bitflags! { 821 | /// BNO055 self-test status bit flags. 822 | #[cfg_attr(not(feature = "defmt-03"), derive(Debug, Clone, Copy, PartialEq, Eq))] 823 | pub struct BNO055SelfTestStatus: u8 { 824 | const ACC_OK = 0b0001; 825 | const MAG_OK = 0b0010; 826 | const GYR_OK = 0b0100; 827 | const SYS_OK = 0b1000; 828 | } 829 | } 830 | 831 | #[derive(Debug, Clone, PartialEq, Eq)] 832 | #[cfg_attr(feature = "defmt-03", derive(defmt::Format))] 833 | pub struct BNO055SystemStatus { 834 | status: BNO055SystemStatusCode, 835 | selftest: Option, 836 | error: BNO055SystemErrorCode, 837 | } 838 | 839 | #[derive(Debug, Clone, Copy, PartialEq, Eq)] 840 | #[cfg_attr(feature = "defmt-03", derive(defmt::Format))] 841 | pub struct BNO055Revision { 842 | pub software: u16, 843 | pub bootloader: u8, 844 | pub accelerometer: u8, 845 | pub magnetometer: u8, 846 | pub gyroscope: u8, 847 | } 848 | 849 | #[derive(Debug, Clone, Copy, PartialEq, Eq)] 850 | #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] 851 | #[cfg_attr(feature = "defmt-03", derive(defmt::Format))] 852 | #[repr(C)] 853 | pub struct BNO055Calibration { 854 | pub acc_offset_x_lsb: u8, 855 | pub acc_offset_x_msb: u8, 856 | pub acc_offset_y_lsb: u8, 857 | pub acc_offset_y_msb: u8, 858 | pub acc_offset_z_lsb: u8, 859 | pub acc_offset_z_msb: u8, 860 | 861 | pub mag_offset_x_lsb: u8, 862 | pub mag_offset_x_msb: u8, 863 | pub mag_offset_y_lsb: u8, 864 | pub mag_offset_y_msb: u8, 865 | pub mag_offset_z_lsb: u8, 866 | pub mag_offset_z_msb: u8, 867 | 868 | pub gyr_offset_x_lsb: u8, 869 | pub gyr_offset_x_msb: u8, 870 | pub gyr_offset_y_lsb: u8, 871 | pub gyr_offset_y_msb: u8, 872 | pub gyr_offset_z_lsb: u8, 873 | pub gyr_offset_z_msb: u8, 874 | 875 | pub acc_radius_lsb: u8, 876 | pub acc_radius_msb: u8, 877 | pub mag_radius_lsb: u8, 878 | pub mag_radius_msb: u8, 879 | } 880 | 881 | /// BNO055's calibration profile size. 882 | pub const BNO055_CALIB_SIZE: usize = core::mem::size_of::(); 883 | 884 | impl BNO055Calibration { 885 | pub fn from_buf(buf: &[u8; BNO055_CALIB_SIZE]) -> BNO055Calibration { 886 | unsafe { core::ptr::read(buf.as_ptr() as *const _) } 887 | } 888 | 889 | pub fn as_bytes(&self) -> &[u8] { 890 | unsafe { 891 | core::slice::from_raw_parts( 892 | (self as *const _) as *const u8, 893 | ::core::mem::size_of::(), 894 | ) 895 | } 896 | } 897 | } 898 | 899 | #[derive(Debug, Clone, Copy, PartialEq, Eq)] 900 | #[cfg_attr(feature = "defmt-03", derive(defmt::Format))] 901 | pub struct BNO055CalibrationStatus { 902 | pub sys: u8, 903 | pub gyr: u8, 904 | pub acc: u8, 905 | pub mag: u8, 906 | } 907 | 908 | bitflags! { 909 | /// Possible BNO055 register map pages. 910 | #[cfg_attr(not(feature = "defmt-03"), derive(Debug, Clone, Copy, PartialEq, Eq))] 911 | pub struct BNO055RegisterPage: u8 { 912 | const PAGE_0 = 0; 913 | const PAGE_1 = 1; 914 | } 915 | } 916 | 917 | bitflags! { 918 | /// Possible BNO055 power modes. 919 | #[cfg_attr(not(feature = "defmt-03"), derive(Debug, Clone, Copy, PartialEq, Eq))] 920 | pub struct BNO055PowerMode: u8 { 921 | const NORMAL = 0b00; 922 | const LOW_POWER = 0b01; 923 | const SUSPEND = 0b10; 924 | } 925 | } 926 | 927 | bitflags! { 928 | /// Possible BNO055 operation modes. 929 | #[cfg_attr(not(feature = "defmt-03"), derive(Debug, Clone, Copy, PartialEq, Eq))] 930 | pub struct BNO055OperationMode: u8 { 931 | const CONFIG_MODE = 0b0000; 932 | const ACC_ONLY = 0b0001; 933 | const MAG_ONLY = 0b0010; 934 | const GYRO_ONLY = 0b0011; 935 | const ACC_MAG = 0b0100; 936 | const ACC_GYRO = 0b0101; 937 | const MAG_GYRO = 0b0110; 938 | const AMG = 0b0111; 939 | const IMU = 0b1000; 940 | const COMPASS = 0b1001; 941 | const M4G = 0b1010; 942 | const NDOF_FMC_OFF = 0b1011; 943 | const NDOF = 0b1100; 944 | } 945 | } 946 | 947 | impl BNO055OperationMode { 948 | fn is_fusion_enabled(&self) -> bool { 949 | matches!( 950 | *self, 951 | Self::IMU | Self::COMPASS | Self::M4G | Self::NDOF_FMC_OFF | Self::NDOF, 952 | ) 953 | } 954 | 955 | fn is_accel_enabled(&self) -> bool { 956 | matches!( 957 | *self, 958 | Self::ACC_ONLY 959 | | Self::ACC_MAG 960 | | Self::ACC_GYRO 961 | | Self::AMG 962 | | Self::IMU 963 | | Self::COMPASS 964 | | Self::M4G 965 | | Self::NDOF_FMC_OFF 966 | | Self::NDOF, 967 | ) 968 | } 969 | 970 | fn is_gyro_enabled(&self) -> bool { 971 | matches!( 972 | *self, 973 | Self::GYRO_ONLY 974 | | Self::ACC_GYRO 975 | | Self::MAG_GYRO 976 | | Self::AMG 977 | | Self::IMU 978 | | Self::NDOF_FMC_OFF 979 | | Self::NDOF, 980 | ) 981 | } 982 | 983 | fn is_mag_enabled(&self) -> bool { 984 | matches!( 985 | *self, 986 | Self::MAG_ONLY 987 | | Self::ACC_MAG 988 | | Self::MAG_GYRO 989 | | Self::AMG 990 | | Self::COMPASS 991 | | Self::M4G 992 | | Self::NDOF_FMC_OFF 993 | | Self::NDOF, 994 | ) 995 | } 996 | } 997 | -------------------------------------------------------------------------------- /src/regs.rs: -------------------------------------------------------------------------------- 1 | #![allow(dead_code)] 2 | 3 | pub(crate) const BNO055_DEFAULT_ADDR: u8 = 0x29; 4 | pub(crate) const BNO055_ALTERNATE_ADDR: u8 = 0x28; 5 | pub const BNO055_ID: u8 = 0xA0; 6 | 7 | pub(crate) const BNO055_PAGE_ID: u8 = 0x07; 8 | 9 | pub(crate) const BNO055_CHIP_ID: u8 = 0x00; 10 | pub(crate) const BNO055_ACC_ID: u8 = 0x01; 11 | pub(crate) const BNO055_MAG_ID: u8 = 0x02; 12 | pub(crate) const BNO055_GYR_ID: u8 = 0x03; 13 | pub(crate) const BNO055_SW_REV_ID_LSB: u8 = 0x04; 14 | pub(crate) const BNO055_SW_REV_ID_MSB: u8 = 0x05; 15 | pub(crate) const BNO055_BL_REV_ID: u8 = 0x06; 16 | 17 | pub(crate) const BNO055_ACC_DATA_X_LSB: u8 = 0x08; 18 | pub(crate) const BNO055_ACC_DATA_X_MSB: u8 = 0x09; 19 | pub(crate) const BNO055_ACC_DATA_Y_LSB: u8 = 0x0A; 20 | pub(crate) const BNO055_ACC_DATA_Y_MSB: u8 = 0x0B; 21 | pub(crate) const BNO055_ACC_DATA_Z_LSB: u8 = 0x0C; 22 | pub(crate) const BNO055_ACC_DATA_Z_MSB: u8 = 0x0D; 23 | 24 | pub(crate) const BNO055_MAG_DATA_X_LSB: u8 = 0x0E; 25 | pub(crate) const BNO055_MAG_DATA_X_MSB: u8 = 0x0F; 26 | pub(crate) const BNO055_MAG_DATA_Y_LSB: u8 = 0x10; 27 | pub(crate) const BNO055_MAG_DATA_Y_MSB: u8 = 0x11; 28 | pub(crate) const BNO055_MAG_DATA_Z_LSB: u8 = 0x12; 29 | pub(crate) const BNO055_MAG_DATA_Z_MSB: u8 = 0x13; 30 | 31 | pub(crate) const BNO055_GYR_DATA_X_LSB: u8 = 0x14; 32 | pub(crate) const BNO055_GYR_DATA_X_MSB: u8 = 0x15; 33 | pub(crate) const BNO055_GYR_DATA_Y_LSB: u8 = 0x16; 34 | pub(crate) const BNO055_GYR_DATA_Y_MSB: u8 = 0x17; 35 | pub(crate) const BNO055_GYR_DATA_Z_LSB: u8 = 0x18; 36 | pub(crate) const BNO055_GYR_DATA_Z_MSB: u8 = 0x19; 37 | 38 | pub(crate) const BNO055_EUL_HEADING_LSB: u8 = 0x1A; 39 | pub(crate) const BNO055_EUL_HEADING_MSB: u8 = 0x1B; 40 | pub(crate) const BNO055_EUL_ROLL_LSB: u8 = 0x1C; 41 | pub(crate) const BNO055_EUL_ROLL_MSB: u8 = 0x1D; 42 | pub(crate) const BNO055_EUL_PITCH_LSB: u8 = 0x1E; 43 | pub(crate) const BNO055_EUL_PITCH_MSB: u8 = 0x1F; 44 | 45 | /// Quaternion data 46 | pub(crate) const BNO055_QUA_DATA_W_LSB: u8 = 0x20; 47 | pub(crate) const BNO055_QUA_DATA_W_MSB: u8 = 0x21; 48 | pub(crate) const BNO055_QUA_DATA_X_LSB: u8 = 0x22; 49 | pub(crate) const BNO055_QUA_DATA_X_MSB: u8 = 0x23; 50 | pub(crate) const BNO055_QUA_DATA_Y_LSB: u8 = 0x24; 51 | pub(crate) const BNO055_QUA_DATA_Y_MSB: u8 = 0x25; 52 | pub(crate) const BNO055_QUA_DATA_Z_LSB: u8 = 0x26; 53 | pub(crate) const BNO055_QUA_DATA_Z_MSB: u8 = 0x27; 54 | 55 | /// Linear acceleration data 56 | pub(crate) const BNO055_LIA_DATA_X_LSB: u8 = 0x28; 57 | pub(crate) const BNO055_LIA_DATA_X_MSB: u8 = 0x29; 58 | pub(crate) const BNO055_LIA_DATA_Y_LSB: u8 = 0x2A; 59 | pub(crate) const BNO055_LIA_DATA_Y_MSB: u8 = 0x2B; 60 | pub(crate) const BNO055_LIA_DATA_Z_LSB: u8 = 0x2C; 61 | pub(crate) const BNO055_LIA_DATA_Z_MSB: u8 = 0x2D; 62 | 63 | /// Gravity vector data 64 | pub(crate) const BNO055_GRV_DATA_X_LSB: u8 = 0x2E; 65 | pub(crate) const BNO055_GRV_DATA_X_MSB: u8 = 0x2F; 66 | pub(crate) const BNO055_GRV_DATA_Y_LSB: u8 = 0x30; 67 | pub(crate) const BNO055_GRV_DATA_Y_MSB: u8 = 0x31; 68 | pub(crate) const BNO055_GRV_DATA_Z_LSB: u8 = 0x32; 69 | pub(crate) const BNO055_GRV_DATA_Z_MSB: u8 = 0x33; 70 | 71 | /// Temperature data 72 | pub(crate) const BNO055_TEMP: u8 = 0x34; 73 | 74 | /// Calibration Status 75 | pub(crate) const BNO055_CALIB_STAT: u8 = 0x35; 76 | 77 | pub(crate) const BNO055_ST_RESULT: u8 = 0x36; 78 | pub(crate) const BNO055_INT_STA: u8 = 0x37; 79 | pub(crate) const BNO055_SYS_CLK_STATUS: u8 = 0x38; 80 | pub(crate) const BNO055_SYS_STATUS: u8 = 0x39; 81 | pub(crate) const BNO055_SYS_ERR: u8 = 0x3A; 82 | pub(crate) const BNO055_UNIT_SEL: u8 = 0x3B; 83 | pub(crate) const BNO055_OPR_MODE: u8 = 0x3D; 84 | pub(crate) const BNO055_PWR_MODE: u8 = 0x3E; 85 | 86 | pub(crate) const BNO055_SYS_TRIGGER: u8 = 0x3F; 87 | pub(crate) const BNO055_SYS_TRIGGER_RST_SYS_BIT: u8 = 0x20; // Reset command 88 | pub(crate) const BNO055_SYS_TRIGGER_SELF_TEST_BIT: u8 = 0b000_0001; // Self-test command 89 | pub(crate) const BNO055_TEMP_SOURCE: u8 = 0x40; 90 | pub(crate) const BNO055_AXIS_MAP_CONFIG: u8 = 0x41; 91 | pub(crate) const BNO055_AXIS_MAP_SIGN: u8 = 0x42; 92 | 93 | /// Calibration data 94 | 95 | pub(crate) const BNO055_ACC_OFFSET_X_LSB: u8 = 0x55; 96 | pub(crate) const BNO055_ACC_OFFSET_X_MSB: u8 = 0x56; 97 | pub(crate) const BNO055_ACC_OFFSET_Y_LSB: u8 = 0x57; 98 | pub(crate) const BNO055_ACC_OFFSET_Y_MSB: u8 = 0x58; 99 | pub(crate) const BNO055_ACC_OFFSET_Z_LSB: u8 = 0x59; 100 | pub(crate) const BNO055_ACC_OFFSET_Z_MSB: u8 = 0x5A; 101 | 102 | pub(crate) const BNO055_MAG_OFFSET_X_LSB: u8 = 0x5B; 103 | pub(crate) const BNO055_MAG_OFFSET_X_MSB: u8 = 0x5C; 104 | pub(crate) const BNO055_MAG_OFFSET_Y_LSB: u8 = 0x5D; 105 | pub(crate) const BNO055_MAG_OFFSET_Y_MSB: u8 = 0x5E; 106 | pub(crate) const BNO055_MAG_OFFSET_Z_LSB: u8 = 0x5F; 107 | pub(crate) const BNO055_MAG_OFFSET_Z_MSB: u8 = 0x60; 108 | 109 | pub(crate) const BNO055_GYR_OFFSET_X_LSB: u8 = 0x61; 110 | pub(crate) const BNO055_GYR_OFFSET_X_MSB: u8 = 0x62; 111 | pub(crate) const BNO055_GYR_OFFSET_Y_LSB: u8 = 0x63; 112 | pub(crate) const BNO055_GYR_OFFSET_Y_MSB: u8 = 0x64; 113 | pub(crate) const BNO055_GYR_OFFSET_Z_LSB: u8 = 0x65; 114 | pub(crate) const BNO055_GYR_OFFSET_Z_MSB: u8 = 0x66; 115 | 116 | pub(crate) const BNO055_ACC_RADIUS_LSB: u8 = 0x67; 117 | pub(crate) const BNO055_ACC_RADIUS_MSB: u8 = 0x68; 118 | pub(crate) const BNO055_MAG_RADIUS_LSB: u8 = 0x69; 119 | pub(crate) const BNO055_MAG_RADIUS_MSB: u8 = 0x6A; 120 | 121 | /// Sensor config 122 | pub(crate) const BNO055_ACC_CONFIG: u8 = 0x08; 123 | -------------------------------------------------------------------------------- /src/std.rs: -------------------------------------------------------------------------------- 1 | use crate::{acc_config, Error}; 2 | use std::{error, fmt}; 3 | 4 | impl error::Error for Error {} 5 | 6 | impl fmt::Display for Error { 7 | fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { 8 | write!(f, "{:?}", self) 9 | } 10 | } 11 | 12 | impl error::Error for acc_config::Error {} 13 | 14 | impl fmt::Display for acc_config::Error { 15 | fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { 16 | write!(f, "{:?}", self) 17 | } 18 | } 19 | --------------------------------------------------------------------------------