diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml index 13961e4..922d875 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -2,9 +2,9 @@ name: Rust on: push: - branches: [ master ] + branches: [master] pull_request: - branches: [ master ] + branches: [master] env: CARGO_TERM_COLOR: always @@ -19,7 +19,7 @@ jobs: submodules: true - uses: dtolnay/rust-toolchain@nightly with: - components: rustfmt, clippy, rust-docs + components: rustfmt, clippy, rust-docs - name: Rustfmt lints run: cargo fmt --all -- --check @@ -28,7 +28,7 @@ jobs: run: cargo clippy --no-deps -- -D warnings - name: Build docs - run: RUSTDOCFLAGS="--cfg docsrs" cargo +nightly doc --all-features --no-deps + run: RUSTDOCFLAGS="--cfg docsrs" cargo +nightly doc --features std,serde,async,defmt --no-deps build: runs-on: ubuntu-latest @@ -38,11 +38,11 @@ jobs: - stable - beta - nightly - - "1.65" # MSRV + - "1.80" # MSRV steps: - name: Checkout Sources - uses: actions/checkout@v4 + uses: actions/checkout@v5 - name: Install Rust (thumbv7em) uses: dtolnay/rust-toolchain@master with: @@ -54,17 +54,16 @@ jobs: run: cargo build --features "std" - name: Build - serde run: cargo build --features "serde" - - name: Build - defmt-03 - run: cargo build --features "defmt-03" - - name: Build - examples - run: cargo build --examples + - name: Build - defmt + run: cargo build --features "defmt" + - name: Build - examples (blocking) + run: cargo build --examples --no-default-features -F blocking - name: Run tests run: cargo test + # Cargo binstall currently installs musl binary and requires additional setup. - name: Install cargo-fuzz - # pre-build binaries - uses: taiki-e/install-action@v2 - with: - tool: cargo-fuzz + if: ${{ matrix.rust == 'nightly' }} + run: cargo +nightly install cargo-fuzz - name: Fuzz (nightly only) if: ${{ matrix.rust == 'nightly' }} run: | diff --git a/Cargo.toml b/Cargo.toml index fbbcdbf..bfe0cd5 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -18,7 +18,7 @@ license-file = "LICENSE" readme = "README.md" # linux-embedded-hal requires 1.65 because of nix -rust-version = "1.65" +rust-version = "1.80" [dependencies] byteorder = { version = "1", default-features = false } @@ -28,16 +28,26 @@ bitflags = "2" num-traits = { version = "0.2.15", default-features = false, features = ["libm"] } num-derive = "0.4.1" -defmt = { version = "0.3", optional = true } +defmt = { version = "1", optional = true } embedded-hal = { version = "1.0" } +embedded-hal-async = { version = "1.0", optional = true } + +maybe-async = "0.2" [features] -default = [] +default = ["async"] std = [] -defmt-03 = ["dep:defmt", "embedded-hal/defmt-03"] +async = ["dep:embedded-hal-async"] +blocking = ["maybe-async/is_sync"] + +defmt = ["dep:defmt", "embedded-hal/defmt-03"] serde = ["dep:serde", "mint/serde"] [dev-dependencies] -linux-embedded-hal = "0.4" +linux-embedded-hal = { version = "0.4", features = ["async-tokio"] } + +[[example]] +name = "calibrate" +required-features = ["blocking"] \ No newline at end of file diff --git a/README.md b/README.md index bec538b..1a6c86f 100644 --- a/README.md +++ b/README.md @@ -40,6 +40,13 @@ If your microcontroller is faster in starting up you might have to delay before By default, this crate is `no_std` compatible. However, you can enable `std` features by enabling the `std` feature flag. At the moment this only adds `std::error::Error` trait implementation for the `Error` type. +### `blocking` +Enables the blocking API but cannot enable it with the `async` features. +Use `default-features = false` to disable `async`. + +### `async` (default) +Enables the async API based on `embedded-hal-async@1` and cannot be enabled if `blocking` is enabled as well. + ### `serde` The `serde` flag adds implementation of `Serialize` / `Deserialize` to `BNO055Calibration`. @@ -47,6 +54,10 @@ The `serde` flag adds implementation of `Serialize` / `Deserialize` to `BNO055Ca **Note:** `serde` itself is `no_std` compatible, however not all serializers are (e.g. `serde-json` is not but `serde-json-core` is), 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). +## `defmt` +The `defmt` flag adds implementation of `defmt::Format` to most structures and enums. + + ## Usage 1. Add a dependency to `Cargo.toml`: diff --git a/examples/calibrate.rs b/examples/calibrate.rs index 5e09971..466b0f0 100644 --- a/examples/calibrate.rs +++ b/examples/calibrate.rs @@ -1,7 +1,17 @@ +//! Run calibration using linux i2c-0 device. +//! `linux_embedded_hal::I2cdev` doesn't support `async`. +//! +//! `cargo run --example calibrate --no-default-features -F blocking` +//! +//! Make sure to run the necessary steps to allow the IMU to auto-calibrate. +//! See Datasheet section 3.11 Page 51, https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf (As of 2021-07-02) use bno055::{BNO055OperationMode, Bno055}; use linux_embedded_hal::{Delay, I2cdev}; use mint::{EulerAngles, Quaternion}; +#[cfg(not(feature = "blocking"))] +compile_error!("You must disable default features and enable 'blocking' to run the example: 'cargo run --example calibrate --no-default-features -F blocking'"); + fn main() { let dev = I2cdev::new("/dev/i2c-0").unwrap(); let mut delay = Delay {}; diff --git a/fuzz/Cargo.toml b/fuzz/Cargo.toml index afbb5f8..6c3711c 100644 --- a/fuzz/Cargo.toml +++ b/fuzz/Cargo.toml @@ -15,6 +15,8 @@ embedded-hal = "1.0" [dependencies.bno055] path = ".." +default-features = false +features = ["blocking"] # Prevent this from interfering with workspaces [workspace] diff --git a/src/acc_config.rs b/src/acc_config.rs index 6dbb496..4ad0b2f 100644 --- a/src/acc_config.rs +++ b/src/acc_config.rs @@ -1,6 +1,10 @@ +use bitflags::bitflags; + use num_derive::FromPrimitive; use num_traits::FromPrimitive; +use crate::BIT_7_RESERVED_MASK; + #[allow(clippy::unusual_byte_groupings)] const ACC_G_RANGE_MASK: u8 = 0b000_000_11; #[allow(clippy::unusual_byte_groupings)] @@ -9,33 +13,65 @@ const ACC_BANDWIDTH_MASK: u8 = 0b000_111_00; const ACC_OPERATION_MODE_MASK: u8 = 0b111_000_00; #[derive(Debug)] -#[cfg_attr(feature = "defmt-03", derive(defmt::Format))] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] #[allow(clippy::enum_variant_names)] pub enum Error { BadAccGRange, BadAccBandwidth, BadAccOperationMode, + BadAccIntSettings, + BadAccAmThreshold, + BadAccHgDuration, + BadAccHgThreshold, + BadAccNmThreshold, + BadAccNmSettings, } -#[derive(Debug, Clone, Copy, FromPrimitive)] -#[cfg_attr(feature = "defmt-03", derive(defmt::Format))] +#[derive(FromPrimitive, Default, Debug, Clone, Copy)] #[repr(u8)] #[allow(clippy::unusual_byte_groupings)] pub enum AccGRange { G2 = 0b000_000_00, + /// 4G is the default value in the register on Reset. + /// + /// Table 3-4: Default sensor settings + /// Sensor Range Bandwidth + /// Accelerometer 4G 62.5 Hz + /// Magnetometer NA 10 Hz + /// Gyroscope 2000 dps 32 Hz + #[default] G4 = 0b000_000_01, G8 = 0b000_000_10, G16 = 0b000_000_11, } -#[derive(Debug, Clone, Copy, FromPrimitive)] -#[cfg_attr(feature = "defmt-03", derive(defmt::Format))] +#[cfg(feature = "defmt")] +impl defmt::Format for AccGRange { + fn format(&self, f: defmt::Formatter) { + match self { + AccGRange::G2 => defmt::write!(f, "2G"), + AccGRange::G4 => defmt::write!(f, "4G"), + AccGRange::G8 => defmt::write!(f, "8G"), + AccGRange::G16 => defmt::write!(f, "16G"), + } + } +} + +#[derive(Default, Debug, Clone, Copy, FromPrimitive)] #[repr(u8)] #[allow(clippy::unusual_byte_groupings)] pub enum AccBandwidth { Hz7_81 = 0b000_000_00, Hz15_63 = 0b000_001_00, Hz31_25 = 0b000_010_00, + /// The default value in the register on Reset. + /// + /// Table 3-4: Default sensor settings + /// Sensor Range Bandwidth + /// Accelerometer 4G 62.5 Hz + /// Magnetometer NA 10 Hz + /// Gyroscope 2000 dps 32 Hz + #[default] Hz62_5 = 0b000_011_00, Hz125 = 0b000_100_00, Hz250 = 0b000_101_00, @@ -43,11 +79,28 @@ pub enum AccBandwidth { Hz1000 = 0b000_111_00, } -#[derive(Debug, Clone, Copy, FromPrimitive)] -#[cfg_attr(feature = "defmt-03", derive(defmt::Format))] +#[cfg(feature = "defmt")] +impl defmt::Format for AccBandwidth { + fn format(&self, f: defmt::Formatter) { + match self { + AccBandwidth::Hz7_81 => defmt::write!(f, "7.81 Hz"), + AccBandwidth::Hz15_63 => defmt::write!(f, "15.63 Hz"), + AccBandwidth::Hz31_25 => defmt::write!(f, "31.25 Hz"), + AccBandwidth::Hz62_5 => defmt::write!(f, "62.5 Hz"), + AccBandwidth::Hz125 => defmt::write!(f, "125 Hz"), + AccBandwidth::Hz250 => defmt::write!(f, "250 Hz"), + AccBandwidth::Hz500 => defmt::write!(f, "500 Hz"), + AccBandwidth::Hz1000 => defmt::write!(f, "1000 Hz"), + } + } +} + +#[derive(Default, Debug, Clone, Copy, FromPrimitive)] #[repr(u8)] #[allow(clippy::unusual_byte_groupings)] pub enum AccOperationMode { + #[default] + /// The default value in the register on Reset. Normal = 0b000_000_00, Suspend = 0b001_000_00, LowPower1 = 0b010_000_00, @@ -56,8 +109,22 @@ pub enum AccOperationMode { DeepSuspend = 0b101_000_00, } -#[derive(Debug, Clone)] -#[cfg_attr(feature = "defmt-03", derive(defmt::Format))] +#[cfg(feature = "defmt")] +impl defmt::Format for AccOperationMode { + fn format(&self, f: defmt::Formatter) { + match self { + AccOperationMode::Normal => defmt::write!(f, "Normal"), + AccOperationMode::Suspend => defmt::write!(f, "Suspend"), + AccOperationMode::LowPower1 => defmt::write!(f, "LowPower1"), + AccOperationMode::Standby => defmt::write!(f, "Standby"), + AccOperationMode::LowPower2 => defmt::write!(f, "LowPower2"), + AccOperationMode::DeepSuspend => defmt::write!(f, "DeepSuspend"), + } + } +} + +#[derive(Default, Debug, Clone)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] pub struct AccConfig { g_range: AccGRange, bandwidth: AccBandwidth, @@ -107,3 +174,119 @@ impl AccConfig { self.operation_mode = operation_mode; } } + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub struct BNO055AccIntSettingsFlags(u8); + +bitflags! { + /// BNO055 accelerometer interrupt settings + impl BNO055AccIntSettingsFlags: u8 { + // pub struct BNO055AccIntSettingsFlags: u8 { + const HG_Z_AXIS = 0b10000000; + const HG_Y_AXIS = 0b01000000; + const HG_X_AXIS = 0b00100000; + const AMNM_Z_AXIS = 0b00010000; + const AMNM_Y_AXIS = 0b00001000; + const AMNM_X_AXIS = 0b00000100; + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +// #[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub struct BNO055AccIntSettings { + pub flags: BNO055AccIntSettingsFlags, + /// `am_dur` in [0, 3] + pub am_dur: u8, +} + +impl num_traits::FromPrimitive for BNO055AccIntSettings { + fn from_u8(regval: u8) -> Option { + Some(Self::from(regval)) + } + + fn from_i64(n: i64) -> Option { + use core::convert::TryFrom; + let regval = u8::try_from(n).ok(); + + regval.map(Self::from) + } + + fn from_u64(n: u64) -> Option { + use core::convert::TryFrom; + let regval = u8::try_from(n).ok(); + + regval.map(Self::from) + } +} + +impl From for BNO055AccIntSettings { + fn from(regval: u8) -> Self { + Self { + flags: BNO055AccIntSettingsFlags::from_bits_truncate(regval), + am_dur: regval & 3, + } + } +} + +impl From for u8 { + fn from(val: BNO055AccIntSettings) -> Self { + let mut dur = val.am_dur; + if dur > 3 { + dur = 3; + } + val.flags.bits() | dur + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub struct BNO055AccNmSettings { + /// `dur` in [0, 0b111111]. Details of how actual duration is calculated in official doc + pub dur: u8, + /// true - no motion, false - slow motion + pub is_no_motion: bool, +} + +impl num_traits::FromPrimitive for BNO055AccNmSettings { + fn from_u8(regval: u8) -> Option { + Some(Self::from(regval)) + } + + fn from_i64(n: i64) -> Option { + use core::convert::TryFrom; + let regval = u8::try_from(n).ok(); + + regval.map(Self::from) + } + + fn from_u64(n: u64) -> Option { + use core::convert::TryFrom; + let regval = u8::try_from(n).ok(); + + regval.map(Self::from) + } +} + +impl From for BNO055AccNmSettings { + fn from(regval: u8) -> Self { + Self { + dur: (regval & 0b01111110) >> 1, + is_no_motion: !matches!(regval & 1, 0), + } + } +} + +impl From for u8 { + fn from(val: BNO055AccNmSettings) -> Self { + let mut dur = val.dur; + if dur > 0b111111 { + dur = 0b111111; + } + let is_no_motion = match val.is_no_motion { + true => 1, + false => 0, + }; + BIT_7_RESERVED_MASK & ((dur << 1) | is_no_motion) + } +} diff --git a/src/gyr_config.rs b/src/gyr_config.rs new file mode 100644 index 0000000..864679d --- /dev/null +++ b/src/gyr_config.rs @@ -0,0 +1,368 @@ +//! BNO055 Gyroscope Configuration + +use core::convert::TryFrom; + +use bitflags::bitflags; +use num_derive::FromPrimitive; +use num_traits::FromPrimitive; + +use crate::BIT_7_RESERVED_MASK; + +#[derive(Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[allow(clippy::enum_variant_names)] +pub enum Error { + InvalidGyrRange, + InvalidGyrBandwidth, + InvalidGyrPowerMode, + BadGyrHrSettings, + BadGyrDurX, + BadGyrHrYSettings, + BadGyrDurY, + BadGyrHrZSettings, + BadGyrDurZ, + BadGyrAmThreshold, + BadGyrAmSettings, +} + +#[derive(Default, Debug, Clone, Copy, PartialEq, Eq, FromPrimitive)] +#[repr(u8)] +pub enum GyrRange { + /// Default value as per "Table 3-7: Default sensor configuration at power-on" + #[default] + Dps2000 = 0b000, + Dps1000 = 0b001, + Dps500 = 0b010, + Dps250 = 0b011, + Dps125 = 0b100, +} + +#[cfg(feature = "defmt")] +impl defmt::Format for GyrRange { + fn format(&self, f: defmt::Formatter) { + match self { + GyrRange::Dps2000 => defmt::write!(f, "2000 dps"), + GyrRange::Dps1000 => defmt::write!(f, "1000 dps"), + GyrRange::Dps500 => defmt::write!(f, "500 dps"), + GyrRange::Dps250 => defmt::write!(f, "250 dps"), + GyrRange::Dps125 => defmt::write!(f, "125 dps"), + } + } +} + +#[derive(Default, Debug, Clone, Copy, PartialEq, Eq, FromPrimitive)] +#[repr(u8)] +pub enum GyrBandwidth { + Hz523 = 0b000, + Hz230 = 0b001, + Hz116 = 0b010, + Hz47 = 0b011, + Hz23 = 0b100, + Hz12 = 0b101, + Hz64 = 0b110, + /// Default value as per "Table 3-7: Default sensor configuration at power-on" + #[default] + Hz32 = 0b111, +} + +#[cfg(feature = "defmt")] +impl defmt::Format for GyrBandwidth { + fn format(&self, f: defmt::Formatter) { + match self { + GyrBandwidth::Hz523 => defmt::write!(f, "523 Hz"), + GyrBandwidth::Hz230 => defmt::write!(f, "230 Hz"), + GyrBandwidth::Hz116 => defmt::write!(f, "116 Hz"), + GyrBandwidth::Hz47 => defmt::write!(f, "47 Hz"), + GyrBandwidth::Hz23 => defmt::write!(f, "23 Hz"), + GyrBandwidth::Hz12 => defmt::write!(f, "12 Hz"), + GyrBandwidth::Hz64 => defmt::write!(f, "64 Hz"), + GyrBandwidth::Hz32 => defmt::write!(f, "32 Hz"), + } + } +} + +#[derive(Default, Debug, Clone, Copy, PartialEq, Eq, FromPrimitive)] +#[repr(u8)] +pub enum GyrPowerMode { + /// Default value as per "Table 3-7: Default sensor configuration at power-on" + #[default] + Normal = 0b000, + FastPowerUp = 0b001, + DeepSuspend = 0b010, + Suspend = 0b011, + AdvancedPowerSave = 0b100, +} + +#[cfg(feature = "defmt")] +impl defmt::Format for GyrPowerMode { + fn format(&self, f: defmt::Formatter) { + match self { + GyrPowerMode::Normal => defmt::write!(f, "Normal"), + GyrPowerMode::FastPowerUp => defmt::write!(f, "Fast Power Up"), + GyrPowerMode::DeepSuspend => defmt::write!(f, "Deep Suspend"), + GyrPowerMode::Suspend => defmt::write!(f, "Suspend"), + GyrPowerMode::AdvancedPowerSave => defmt::write!(f, "Advanced Power Save"), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub struct GyrConfig { + range: GyrRange, + bandwidth: GyrBandwidth, + power_mode: GyrPowerMode, +} + +impl Default for GyrConfig { + fn default() -> Self { + Self { + range: GyrRange::Dps2000, + bandwidth: GyrBandwidth::Hz32, + power_mode: GyrPowerMode::Normal, + } + } +} + +impl GyrConfig { + pub fn new() -> Self { + Self::default() + } + + pub fn set_range(&mut self, range: GyrRange) { + self.range = range; + } + + pub fn range(&self) -> GyrRange { + self.range + } + + pub fn set_bandwidth(&mut self, bw: GyrBandwidth) { + self.bandwidth = bw; + } + + pub fn bandwidth(&self) -> GyrBandwidth { + self.bandwidth + } + + pub fn set_power_mode(&mut self, mode: GyrPowerMode) { + self.power_mode = mode; + } + + pub fn power_mode(&self) -> GyrPowerMode { + self.power_mode + } + + pub fn to_bits(&self) -> (u8, u8) { + let bits0 = ((self.bandwidth as u8) << 3) | (self.range as u8); + let bits1 = self.power_mode as u8; + (bits0, bits1) + } + + pub fn from_bits(bits0: u8, bits1: u8) -> Result { + let range = GyrRange::from_u8(bits0 & 0b111).ok_or(Error::InvalidGyrRange)?; + let bandwidth = + GyrBandwidth::from_u8((bits0 >> 3) & 0b111).ok_or(Error::InvalidGyrBandwidth)?; + let power_mode = GyrPowerMode::from_u8(bits1 & 0b111).ok_or(Error::InvalidGyrPowerMode)?; + Ok(Self { + range, + bandwidth, + power_mode, + }) + } +} + +bitflags! { + /// BNO055 gyroscope interrupt settings + #[cfg_attr(not(feature = "defmt"), derive(Debug, Clone, Copy, PartialEq, Eq))] + pub struct BNO055GyrIntSettings: u8 { + const HR_FILT = 0b10000000; + const AM_FILT = 0b01000000; + const HR_Z_AXIS = 0b00100000; + const HR_Y_AXIS = 0b00010000; + const HR_X_AXIS = 0b00001000; + const AM_Z_AXIS = 0b00000100; + const AM_Y_AXIS = 0b00000010; + const AM_X_AXIS = 0b00000001; + } +} + +impl TryFrom for BNO055GyrIntSettings { + type Error = (); + + fn try_from(value: u8) -> Result { + Self::from_bits(value).ok_or(()) + } +} + +impl num_traits::FromPrimitive for BNO055GyrIntSettings { + fn from_u8(regval: u8) -> Option { + Self::try_from(regval).ok() + } + + fn from_i64(n: i64) -> Option { + u8::try_from(n) + .ok() + .and_then(|val| Self::try_from(val).ok()) + } + + fn from_u64(n: u64) -> Option { + u8::try_from(n) + .ok() + .and_then(|val| Self::try_from(val).ok()) + } +} + +/// Gyroscope High Rate interrupt settings +#[derive(Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub struct BNO055GyrHrSettings { + /// `hysteresis` in [0, 0b11]. Actual value is `hysteresis` * base-unit based on gyroscope range set in GYR_CONFIG + pub hysteresis: u8, + /// `threshold` in [0, 0b11111]. Actual value is `threshold` * base-unit based on gyroscope range set in GYR_CONFIG + pub threshold: u8, +} + +impl num_traits::FromPrimitive for BNO055GyrHrSettings { + fn from_u8(regval: u8) -> Option { + Some(Self::from(regval)) + } + + fn from_i64(n: i64) -> Option { + use core::convert::TryFrom; + let regval = u8::try_from(n).ok(); + + regval.map(Self::from) + } + + fn from_u64(n: u64) -> Option { + use core::convert::TryFrom; + let regval = u8::try_from(n).ok(); + + regval.map(Self::from) + } +} + +impl From for BNO055GyrHrSettings { + fn from(regval: u8) -> Self { + Self { + hysteresis: (regval & 0b01100000) >> 5, + threshold: regval & 0b00011111, + } + } +} + +impl From for u8 { + fn from(val: BNO055GyrHrSettings) -> Self { + let mut hysteresis = val.hysteresis; + if hysteresis > 0b11 { + hysteresis = 0b11; + } + let mut threshold = val.threshold; + if threshold > 0b11111 { + threshold = 0b11111; + } + BIT_7_RESERVED_MASK & (hysteresis << 5 | threshold) + } +} + +/// Gyroscope Any Motion interrupt settings +#[derive(Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub struct BNO055GyrAmSettings { + pub awake_duration: GyrAmSamplesAwake, + /// `slope_samples` in [0, 0b11]. Actual value is (`slope_samples` + 1) * 4 + pub slope_samples: u8, +} + +impl num_traits::FromPrimitive for BNO055GyrAmSettings { + fn from_u8(regval: u8) -> Option { + Some(Self::from(regval)) + } + + fn from_i64(n: i64) -> Option { + use core::convert::TryFrom; + let regval = u8::try_from(n).ok(); + + regval.map(Self::from) + } + + fn from_u64(n: u64) -> Option { + use core::convert::TryFrom; + let regval = u8::try_from(n).ok(); + + regval.map(Self::from) + } +} + +impl From for BNO055GyrAmSettings { + fn from(regval: u8) -> Self { + Self { + awake_duration: ((regval >> 2) & 3).into(), + slope_samples: regval & 3, + } + } +} + +impl From for u8 { + fn from(val: BNO055GyrAmSettings) -> Self { + let mut slope_samples = val.slope_samples; + if slope_samples > 0b11 { + slope_samples = 0b11; + } + let awake_duration: u8 = val.awake_duration.into(); + 0b00001111 & (awake_duration | slope_samples) + } +} + +#[derive(Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum GyrAmSamplesAwake { + Samples8, + Samples16, + Samples32, + Samples64, +} + +impl From for GyrAmSamplesAwake { + fn from(regval: u8) -> Self { + match regval { + 0 => Self::Samples8, + 1 => Self::Samples16, + 2 => Self::Samples32, + 3 => Self::Samples64, + _ => Self::Samples8, // TODO: handle error case? + } + } +} + +impl From for u8 { + fn from(val: GyrAmSamplesAwake) -> Self { + match val { + GyrAmSamplesAwake::Samples8 => 0, + GyrAmSamplesAwake::Samples16 => 1, + GyrAmSamplesAwake::Samples32 => 2, + GyrAmSamplesAwake::Samples64 => 3, + } + } +} + +impl num_traits::FromPrimitive for GyrAmSamplesAwake { + fn from_u8(regval: u8) -> Option { + Some(Self::from(regval)) + } + + fn from_i64(n: i64) -> Option { + use core::convert::TryFrom; + let regval = u8::try_from(n).ok(); + + regval.map(Self::from) + } + + fn from_u64(n: u64) -> Option { + use core::convert::TryFrom; + let regval = u8::try_from(n).ok(); + + regval.map(Self::from) + } +} diff --git a/src/lib.rs b/src/lib.rs index bfacf22..85922e4 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,35 +1,70 @@ -#![doc(html_root_url = "https://docs.rs/bno055/0.4.0")] +#![doc(html_root_url = "https://docs.rs/bno055/0.5.0")] // Version bump #![cfg_attr(not(feature = "std"), no_std)] #![allow(clippy::bad_bit_mask)] //! Bosch Sensortec BNO055 9-axis IMU sensor driver. -//! Datasheet: https://ae-bst.resource.bosch.com/media/_tech/media/datasheets/BST-BNO055-DS000.pdf +//! +//! Datasheet: + +#[cfg(all(feature = "blocking", feature = "async"))] +compile_error!("Features 'blocking' and 'async' cannot be enabled at the same time."); + +// Use maybe_async to toggle between blocking and async implementations +use maybe_async::maybe_async; + +// Cfg-gate the HAL traits to select the correct ones based on the "async" feature +#[cfg(not(feature = "async"))] use embedded_hal::{ delay::DelayNs, i2c::{I2c, SevenBitAddress}, }; -#[cfg(not(feature = "defmt-03"))] -use bitflags::bitflags; -#[cfg(feature = "defmt-03")] -use defmt::bitflags; +#[cfg(feature = "async")] +use embedded_hal_async::{ + delay::DelayNs, + i2c::{I2c, SevenBitAddress}, +}; +use bitflags::bitflags; use byteorder::{ByteOrder, LittleEndian}; pub use mint; +use num_traits::FromPrimitive; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; -mod acc_config; +pub mod acc_config; +pub mod gyr_config; +pub mod mag_config; mod regs; #[cfg(feature = "std")] -mod std; +pub mod std; -pub use acc_config::{AccBandwidth, AccConfig, AccGRange, AccOperationMode}; +#[doc(inline)] +pub use acc_config::{ + AccBandwidth, AccConfig, AccGRange, AccOperationMode, BNO055AccIntSettings, BNO055AccNmSettings, +}; +#[doc(inline)] +pub use gyr_config::{ + BNO055GyrAmSettings, BNO055GyrHrSettings, BNO055GyrIntSettings, GyrAmSamplesAwake, + GyrBandwidth, GyrConfig, GyrPowerMode, GyrRange, +}; +#[doc(inline)] +pub use mag_config::{MagConfig, MagDataRate, MagOperationMode, MagPowerMode}; +#[doc(inline)] pub use regs::BNO055_ID; +pub(crate) const BIT_7_RESERVED_MASK: u8 = 0b01111111; + +/// 1 m/s^2 = 100 lsb +pub const ACCEL_SCALING: f32 = 1f32 / 100f32; +/// 1 deg/s = 16 lsb +pub const GYRO_SCALING: f32 = 1f32 / 16f32; +/// 1 uT = 16 lsb +pub const MAG_SCALING: f32 = 1f32 / 16f32; + /// All possible errors in this crate #[derive(Debug)] -#[cfg_attr(feature = "defmt-03", derive(defmt::Format))] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] pub enum Error { /// I2C bus error I2c(E), @@ -42,15 +77,22 @@ pub enum Error { /// Accelerometer configuration error AccConfig(acc_config::Error), + + /// Gyroscope configuration error + GyroConfig(gyr_config::Error), + + /// Magnetometer configuration error + MagConfig(mag_config::Error), } -#[cfg_attr(feature = "defmt-03", derive(defmt::Format))] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] pub struct Bno055 { i2c: I, pub mode: BNO055OperationMode, use_default_addr: bool, } +#[maybe_async] impl Bno055 where I: I2c, @@ -73,7 +115,6 @@ where /// Enables use of alternative I2C address `regs::BNO055_ALTERNATE_ADDR`. pub fn with_alternative_address(mut self) -> Self { self.use_default_addr = false; - self } @@ -84,195 +125,150 @@ where /// - Sets BNO055 to `CONFIG` mode /// - Sets BNO055's power mode to `NORMAL` /// - Clears `SYS_TRIGGER` register - /// - /// # Usage Example - /// - /// ```rust - /// // use your_chip_hal::{I2c, Delay}; // <- import your chip's I2c and Delay - /// use bno055::Bno055; - /// # - /// # // All of this is needed for example to work: - /// # use bno055::BNO055_ID; - /// # use embedded_hal::delay::DelayNs; - /// # use embedded_hal::i2c::{I2c as I2cTrait, Operation, Error, ErrorType, ErrorKind}; - /// # struct Delay {} - /// # impl Delay { pub fn new() -> Self { Delay{ } }} - /// # impl DelayNs for Delay { - /// # fn delay_ns(&mut self, ms: u32) { - /// # // no-op for example purposes - /// # } - /// # } - /// # struct I2c {} - /// # impl I2c { pub fn new() -> Self { I2c { } }} - /// # #[derive(Debug)] - /// # struct DummyError {} - /// # impl Error for DummyError { fn kind(&self) -> ErrorKind { ErrorKind::Other } } - /// # impl ErrorType for I2c { type Error = DummyError; } - /// # // 3 calls are made, 2 Writes and 1 Write/Read. We want to mock the 3rd call's read. - /// # 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(()) } } - /// # - /// # // Actual example: - /// let mut delay = Delay::new(/* ... */); - /// let mut i2c = I2c::new(/* ... */); - /// let mut bno055 = Bno055::new(i2c); - /// bno055.init(&mut delay)?; - /// # Result::<(), bno055::Error>::Ok(()) - /// ``` - pub fn init(&mut self, delay: &mut dyn DelayNs) -> Result<(), Error> { - self.set_page(BNO055RegisterPage::PAGE_0)?; - - let id = self.id()?; + pub async fn init(&mut self, delay: &mut D) -> Result<(), Error> { + self.set_page(BNO055RegisterPage::PAGE_0).await?; + + let id = self.id().await?; if id != regs::BNO055_ID { return Err(Error::InvalidChipId(id)); } - self.soft_reset(delay)?; - self.set_mode(BNO055OperationMode::CONFIG_MODE, delay)?; - self.set_power_mode(BNO055PowerMode::NORMAL)?; - self.write_u8(regs::BNO055_SYS_TRIGGER, 0x00) - .map_err(Error::I2c)?; + self.soft_reset(delay).await?; + self.set_mode(BNO055OperationMode::CONFIG_MODE, delay) + .await?; + self.set_power_mode(BNO055PowerMode::NORMAL).await?; + self.write_u8(regs::BNO055_SYS_TRIGGER, 0x00).await?; Ok(()) } /// Resets the BNO055, initializing the register map to default values. /// More in section 3.2. - pub fn soft_reset(&mut self, delay: &mut dyn DelayNs) -> Result<(), Error> { - self.set_page(BNO055RegisterPage::PAGE_0)?; - + pub async fn soft_reset(&mut self, delay: &mut D) -> Result<(), Error> { + self.set_page(BNO055RegisterPage::PAGE_0).await?; self.write_u8( regs::BNO055_SYS_TRIGGER, - regs::BNO055_SYS_TRIGGER_RST_SYS_BIT, + BNO055SystemTrigger::RST_SYS.bits(), ) - .map_err(Error::I2c)?; + .await?; // As per table 1.2 - delay.delay_ms(650); + delay.delay_ms(650).await; + Ok(()) + } + + /// Run Self-test on the BNO055 + /// + /// See section 3.9.2 Built-In Self-Test (BIST) + pub async fn self_test(&mut self, delay: &mut D) -> Result<(), Error> { + self.set_page(BNO055RegisterPage::PAGE_0).await?; + let prev = self.mode; + self.set_mode(BNO055OperationMode::CONFIG_MODE, delay) + .await?; + self.write_u8( + regs::BNO055_SYS_TRIGGER, + BNO055SystemTrigger::SELF_TEST.bits(), + ) + .await?; + self.set_mode(prev, delay).await?; Ok(()) } - /// Sets the operating mode, see [BNO055OperationMode](enum.BNO055OperationMode.html). + /// Sets the operating mode, see [BNO055OperationMode]. /// See section 3.3. - pub fn set_mode( + pub async fn set_mode( &mut self, mode: BNO055OperationMode, - delay: &mut dyn DelayNs, + delay: &mut D, ) -> Result<(), Error> { if self.mode != mode { - self.set_page(BNO055RegisterPage::PAGE_0)?; - + self.set_page(BNO055RegisterPage::PAGE_0).await?; self.mode = mode; - - self.write_u8(regs::BNO055_OPR_MODE, mode.bits()) - .map_err(Error::I2c)?; - + self.write_u8(regs::BNO055_OPR_MODE, mode.bits()).await?; // Table 3-6 says 19ms to switch to CONFIG_MODE - delay.delay_ms(19); + delay.delay_ms(19).await; } - Ok(()) } /// Sets the power mode, see [BNO055PowerMode](enum.BNO055PowerMode.html) /// See section 3.2 - pub fn set_power_mode(&mut self, mode: BNO055PowerMode) -> Result<(), Error> { - self.set_page(BNO055RegisterPage::PAGE_0)?; - - self.write_u8(regs::BNO055_PWR_MODE, mode.bits()) - .map_err(Error::I2c)?; - + pub async fn set_power_mode(&mut self, mode: BNO055PowerMode) -> Result<(), Error> { + self.set_page(BNO055RegisterPage::PAGE_0).await?; + self.write_u8(regs::BNO055_PWR_MODE, mode.bits()).await?; Ok(()) } /// Returns BNO055's power mode. - pub fn power_mode(&mut self) -> Result> { - self.set_page(BNO055RegisterPage::PAGE_0)?; - - let mode = self.read_u8(regs::BNO055_PWR_MODE).map_err(Error::I2c)?; - + pub async fn power_mode(&mut self) -> Result> { + self.set_page(BNO055RegisterPage::PAGE_0).await?; + let mode = self.read_u8(regs::BNO055_PWR_MODE).await?; Ok(BNO055PowerMode::from_bits_truncate(mode)) } /// Enables/Disables usage of external 32k crystal. - pub fn set_external_crystal( + pub async fn set_external_crystal( &mut self, ext: bool, - delay: &mut dyn DelayNs, + delay: &mut D, ) -> Result<(), Error> { - self.set_page(BNO055RegisterPage::PAGE_0)?; - + self.set_page(BNO055RegisterPage::PAGE_0).await?; let prev = self.mode; - self.set_mode(BNO055OperationMode::CONFIG_MODE, delay)?; - self.write_u8(regs::BNO055_SYS_TRIGGER, if ext { 0x80 } else { 0x00 }) - .map_err(Error::I2c)?; - - self.set_mode(prev, delay)?; - + self.set_mode(BNO055OperationMode::CONFIG_MODE, delay) + .await?; + let value = if ext { + BNO055SystemTrigger::EXT_CLK_SEL.bits() + } else { + 0x00 + }; + self.write_u8(regs::BNO055_SYS_TRIGGER, value).await?; + self.set_mode(prev, delay).await?; Ok(()) } /// Configures axis remap of the device. - pub fn set_axis_remap(&mut self, remap: AxisRemap) -> Result<(), Error> { - self.set_page(BNO055RegisterPage::PAGE_0)?; - + pub async fn set_axis_remap(&mut self, remap: AxisRemap) -> Result<(), Error> { + self.set_page(BNO055RegisterPage::PAGE_0).await?; let remap_value = (remap.x.bits() & 0b11) | ((remap.y.bits() & 0b11) << 2) | ((remap.z.bits() & 0b11) << 4); - self.write_u8(regs::BNO055_AXIS_MAP_CONFIG, remap_value) - .map_err(Error::I2c)?; - + .await?; Ok(()) } /// Returns axis remap of the device. - pub fn axis_remap(&mut self) -> Result> { - self.set_page(BNO055RegisterPage::PAGE_0)?; - - let value = self - .read_u8(regs::BNO055_AXIS_MAP_CONFIG) - .map_err(Error::I2c)?; - + pub async fn axis_remap(&mut self) -> Result> { + self.set_page(BNO055RegisterPage::PAGE_0).await?; + let value = self.read_u8(regs::BNO055_AXIS_MAP_CONFIG).await?; let remap = AxisRemap { x: BNO055AxisConfig::from_bits_truncate(value & 0b11), y: BNO055AxisConfig::from_bits_truncate((value >> 2) & 0b11), z: BNO055AxisConfig::from_bits_truncate((value >> 4) & 0b11), }; - Ok(remap) } /// Configures device's axes sign: positive or negative. - pub fn set_axis_sign(&mut self, sign: BNO055AxisSign) -> Result<(), Error> { - self.set_page(BNO055RegisterPage::PAGE_0)?; - + pub async fn set_axis_sign(&mut self, sign: BNO055AxisSign) -> Result<(), Error> { + self.set_page(BNO055RegisterPage::PAGE_0).await?; self.write_u8(regs::BNO055_AXIS_MAP_SIGN, sign.bits()) - .map_err(Error::I2c)?; - + .await?; Ok(()) } /// Return device's axes sign. - pub fn axis_sign(&mut self) -> Result> { - self.set_page(BNO055RegisterPage::PAGE_0)?; - - let value = self - .read_u8(regs::BNO055_AXIS_MAP_SIGN) - .map_err(Error::I2c)?; - + pub async fn axis_sign(&mut self) -> Result> { + self.set_page(BNO055RegisterPage::PAGE_0).await?; + let value = self.read_u8(regs::BNO055_AXIS_MAP_SIGN).await?; Ok(BNO055AxisSign::from_bits_truncate(value)) } - /// Gets the revision of software, bootloader, accelerometer, magnetometer, and gyroscope of - /// the BNO055 device. - pub fn get_revision(&mut self) -> Result> { - self.set_page(BNO055RegisterPage::PAGE_0)?; - + /// Gets the revision of software, bootloader, accelerometer, magnetometer, and gyroscope. + pub async fn get_revision(&mut self) -> Result> { + self.set_page(BNO055RegisterPage::PAGE_0).await?; let mut buf: [u8; 6] = [0; 6]; - - self.read_bytes(regs::BNO055_ACC_ID, &mut buf) - .map_err(Error::I2c)?; - + self.read_bytes(regs::BNO055_ACC_ID, &mut buf).await?; Ok(BNO055Revision { software: LittleEndian::read_u16(&buf[3..5]), bootloader: buf[5], @@ -283,38 +279,30 @@ where } /// Returns device's system status. - pub fn get_system_status( + pub async fn get_system_status( &mut self, do_selftest: bool, - delay: &mut dyn DelayNs, + delay: &mut D, ) -> Result> { - self.set_page(BNO055RegisterPage::PAGE_0)?; + self.set_page(BNO055RegisterPage::PAGE_0).await?; let selftest = if do_selftest { let prev = self.mode; - self.set_mode(BNO055OperationMode::CONFIG_MODE, delay)?; - - let sys_trigger = self.read_u8(regs::BNO055_SYS_TRIGGER).map_err(Error::I2c)?; - + self.set_mode(BNO055OperationMode::CONFIG_MODE, delay) + .await?; + let sys_trigger = self.read_u8(regs::BNO055_SYS_TRIGGER).await?; self.write_u8(regs::BNO055_SYS_TRIGGER, sys_trigger | 0x1) - .map_err(Error::I2c)?; - - // Wait for self-test result - for _ in 0..4 { - delay.delay_ms(255); - } - - let result = self.read_u8(regs::BNO055_ST_RESULT).map_err(Error::I2c)?; - - self.set_mode(prev, delay)?; // Restore previous mode - + .await?; + delay.delay_ms(1000).await; // Wait for self-test result + let result = self.read_u8(regs::BNO055_ST_RESULT).await?; + self.set_mode(prev, delay).await?; // Restore previous mode Some(BNO055SelfTestStatus::from_bits_truncate(result)) } else { None }; - let status = self.read_u8(regs::BNO055_SYS_STATUS).map_err(Error::I2c)?; - let error = self.read_u8(regs::BNO055_SYS_ERR).map_err(Error::I2c)?; + let status = self.read_u8(regs::BNO055_SYS_STATUS).await?; + let error = self.read_u8(regs::BNO055_SYS_ERR).await?; Ok(BNO055SystemStatus { status: BNO055SystemStatusCode::from_bits_truncate(status), @@ -324,347 +312,928 @@ where } /// Gets a quaternion (`mint::Quaternion`) reading from the BNO055. - /// Available only in sensor fusion modes. - pub fn quaternion(&mut self) -> Result, Error> { - self.set_page(BNO055RegisterPage::PAGE_0)?; - - // Device should be in fusion mode to be able to produce quaternions - if self.mode.is_fusion_enabled() { - let mut buf: [u8; 8] = [0; 8]; - self.read_bytes(regs::BNO055_QUA_DATA_W_LSB, &mut buf) - .map_err(Error::I2c)?; - - let w = LittleEndian::read_i16(&buf[0..2]); - let x = LittleEndian::read_i16(&buf[2..4]); - let y = LittleEndian::read_i16(&buf[4..6]); - let z = LittleEndian::read_i16(&buf[6..8]); - - let scale = 1.0 / ((1 << 14) as f32); - - let x = x as f32 * scale; - let y = y as f32 * scale; - let z = z as f32 * scale; - let w = w as f32 * scale; - - let quat = mint::Quaternion { - v: mint::Vector3 { x, y, z }, - s: w, - }; - - Ok(quat) - } else { - Err(Error::InvalidMode) + pub async fn quaternion(&mut self) -> Result, Error> { + if !self.mode.is_fusion_enabled() { + return Err(Error::InvalidMode); } + self.set_page(BNO055RegisterPage::PAGE_0).await?; + let mut buf: [u8; 8] = [0; 8]; + self.read_bytes(regs::BNO055_QUA_DATA_W_LSB, &mut buf) + .await?; + let w = LittleEndian::read_i16(&buf[0..2]); + let x = LittleEndian::read_i16(&buf[2..4]); + let y = LittleEndian::read_i16(&buf[4..6]); + let z = LittleEndian::read_i16(&buf[6..8]); + let scale = 1.0 / ((1 << 14) as f32); + Ok(mint::Quaternion { + v: mint::Vector3 { + x: x as f32 * scale, + y: y as f32 * scale, + z: z as f32 * scale, + }, + s: w as f32 * scale, + }) } /// Get Euler angles representation of heading in degrees. - /// Euler angles is represented as (`roll`, `pitch`, `yaw/heading`). - /// Available only in sensor fusion modes. - pub fn euler_angles(&mut self) -> Result, Error> { - self.set_page(BNO055RegisterPage::PAGE_0)?; - - // Device should be in fusion mode to be able to produce Euler angles - if self.mode.is_fusion_enabled() { - let mut buf: [u8; 6] = [0; 6]; - - self.read_bytes(regs::BNO055_EUL_HEADING_LSB, &mut buf) - .map_err(Error::I2c)?; - - let heading = LittleEndian::read_i16(&buf[0..2]) as f32; - let roll = LittleEndian::read_i16(&buf[2..4]) as f32; - let pitch = LittleEndian::read_i16(&buf[4..6]) as f32; - - let scale = 1f32 / 16f32; // 1 degree = 16 LSB - - let rot = mint::EulerAngles::from([roll * scale, pitch * scale, heading * scale]); - - Ok(rot) - } else { - Err(Error::InvalidMode) + pub async fn euler_angles(&mut self) -> Result, Error> { + if !self.mode.is_fusion_enabled() { + return Err(Error::InvalidMode); } + self.set_page(BNO055RegisterPage::PAGE_0).await?; + let mut buf: [u8; 6] = [0; 6]; + self.read_bytes(regs::BNO055_EUL_HEADING_LSB, &mut buf) + .await?; + let heading = LittleEndian::read_i16(&buf[0..2]) as f32; + let roll = LittleEndian::read_i16(&buf[2..4]) as f32; + let pitch = LittleEndian::read_i16(&buf[4..6]) as f32; + let scale = 1f32 / 16f32; + Ok(mint::EulerAngles::from([ + roll * scale, + pitch * scale, + heading * scale, + ])) } /// Get calibration status - pub fn get_calibration_status(&mut self) -> Result> { - self.set_page(BNO055RegisterPage::PAGE_0)?; - - let status = self.read_u8(regs::BNO055_CALIB_STAT).map_err(Error::I2c)?; - - let sys = (status >> 6) & 0b11; - let gyr = (status >> 4) & 0b11; - let acc = (status >> 2) & 0b11; - let mag = status & 0b11; - - Ok(BNO055CalibrationStatus { sys, gyr, acc, mag }) + pub async fn get_calibration_status(&mut self) -> Result> { + self.set_page(BNO055RegisterPage::PAGE_0).await?; + let status = self.read_u8(regs::BNO055_CALIB_STAT).await?; + Ok(BNO055CalibrationStatus { + sys: (status >> 6) & 0b11, + gyr: (status >> 4) & 0b11, + acc: (status >> 2) & 0b11, + mag: status & 0b11, + }) } /// Checks whether device is fully calibrated or not. - pub fn is_fully_calibrated(&mut self) -> Result> { - let status = self.get_calibration_status()?; + pub async fn is_fully_calibrated(&mut self) -> Result> { + let status = self.get_calibration_status().await?; Ok(status.mag == 3 && status.gyr == 3 && status.acc == 3 && status.sys == 3) } /// Reads current calibration profile of the device. - pub fn calibration_profile( + pub async fn calibration_profile( &mut self, - delay: &mut dyn DelayNs, + delay: &mut D, ) -> Result> { - self.set_page(BNO055RegisterPage::PAGE_0)?; - + self.set_page(BNO055RegisterPage::PAGE_0).await?; let prev_mode = self.mode; - self.set_mode(BNO055OperationMode::CONFIG_MODE, delay)?; - + self.set_mode(BNO055OperationMode::CONFIG_MODE, delay) + .await?; let mut buf: [u8; BNO055_CALIB_SIZE] = [0; BNO055_CALIB_SIZE]; - self.read_bytes(regs::BNO055_ACC_OFFSET_X_LSB, &mut buf[..]) - .map_err(Error::I2c)?; - + .await?; let res = BNO055Calibration::from_buf(&buf); - - self.set_mode(prev_mode, delay)?; - + self.set_mode(prev_mode, delay).await?; Ok(res) } /// Sets current calibration profile. - pub fn set_calibration_profile( + pub async fn set_calibration_profile( &mut self, calib: BNO055Calibration, - delay: &mut dyn DelayNs, + delay: &mut D, ) -> Result<(), Error> { - self.set_page(BNO055RegisterPage::PAGE_0)?; - + self.set_page(BNO055RegisterPage::PAGE_0).await?; let prev_mode = self.mode; - self.set_mode(BNO055OperationMode::CONFIG_MODE, delay)?; - + self.set_mode(BNO055OperationMode::CONFIG_MODE, delay) + .await?; let buf_profile = calib.as_bytes(); - - // Combine register address and profile into single buffer - let buf_reg = [regs::BNO055_ACC_OFFSET_X_LSB; 1]; - let mut buf_with_reg = [0u8; 1 + BNO055_CALIB_SIZE]; - for (to, from) in buf_with_reg - .iter_mut() - .zip(buf_reg.iter().chain(buf_profile.iter())) - { - *to = *from - } - - self.i2c - .write(self.i2c_addr(), &buf_with_reg[..]) - .map_err(Error::I2c)?; - - self.set_mode(prev_mode, delay)?; - + self.write_bytes(regs::BNO055_ACC_OFFSET_X_LSB, buf_profile) + .await?; + self.set_mode(prev_mode, delay).await?; Ok(()) } /// Returns device's factory-programmed and constant chip ID. - /// This ID is device model ID and not a BNO055's unique ID, whic is stored in different register. - pub fn id(&mut self) -> Result> { - self.set_page(BNO055RegisterPage::PAGE_0)?; - self.read_u8(regs::BNO055_CHIP_ID).map_err(Error::I2c) + pub async fn id(&mut self) -> Result> { + self.set_page(BNO055RegisterPage::PAGE_0).await?; + self.read_u8(regs::BNO055_CHIP_ID).await } /// Returns device's operation mode. - pub fn get_mode(&mut self) -> Result> { - self.set_page(BNO055RegisterPage::PAGE_0)?; - - let mode = self.read_u8(regs::BNO055_OPR_MODE).map_err(Error::I2c)?; + pub async fn get_mode(&mut self) -> Result> { + self.set_page(BNO055RegisterPage::PAGE_0).await?; + let mode = self.read_u8(regs::BNO055_OPR_MODE).await?; let mode = BNO055OperationMode::from_bits_truncate(mode); self.mode = mode; - Ok(mode) } - /// Checks whether the device is in Sensor Fusion mode or not. - pub fn is_in_fusion_mode(&mut self) -> Result> { - Ok(self.mode.is_fusion_enabled()) + /// Checks whether the device is in Sensor Fusion mode or not by reading from the device. + pub async fn is_in_fusion_mode(&mut self) -> Result> { + let mode = self.get_mode().await?; + Ok(mode.is_fusion_enabled()) } - pub fn get_acc_config(&mut self) -> Result> { - self.set_page(BNO055RegisterPage::PAGE_1)?; - - let bits = self.read_u8(regs::BNO055_ACC_CONFIG).map_err(Error::I2c)?; - - let acc_config = AccConfig::try_from_bits(bits).map_err(Error::AccConfig)?; - - Ok(acc_config) + /// Returns the current accelerometer config + pub async fn get_acc_config(&mut self) -> Result> { + self.set_page(BNO055RegisterPage::PAGE_1).await?; + let bits = self.read_u8(regs::BNO055_ACC_CONFIG).await?; + AccConfig::try_from_bits(bits).map_err(Error::AccConfig) } - pub fn set_acc_config(&mut self, acc_config: &AccConfig) -> Result<(), Error> { - self.set_page(BNO055RegisterPage::PAGE_1)?; - + /// Sets the accelerometer config + pub async fn set_acc_config( + &mut self, + acc_config: &AccConfig, + delay: &mut D, + ) -> Result<(), Error> { + let prev_mode = self.mode; + self.set_mode(BNO055OperationMode::CONFIG_MODE, delay) + .await?; + self.set_page(BNO055RegisterPage::PAGE_1).await?; self.write_u8(regs::BNO055_ACC_CONFIG, acc_config.bits()) - .map_err(Error::I2c)?; + .await?; + self.set_mode(prev_mode, delay).await?; Ok(()) } - /// Sets current register map page. - fn set_page(&mut self, page: BNO055RegisterPage) -> Result<(), Error> { - self.write_u8(regs::BNO055_PAGE_ID, page.bits()) - .map_err(Error::I2c)?; - - Ok(()) + /// Returns the current gyroscope config + pub async fn get_gyr_config(&mut self) -> Result> { + self.set_page(BNO055RegisterPage::PAGE_1).await?; + let bits0 = self.read_u8(regs::BNO055_GYR_CONFIG_0).await?; + let bits1 = self.read_u8(regs::BNO055_GYR_CONFIG_1).await?; + GyrConfig::from_bits(bits0, bits1).map_err(Error::GyroConfig) } - /// Reads a vector of sensor data from the device. - fn read_vec_raw(&mut self, reg: u8) -> Result, Error> { - let mut buf: [u8; 6] = [0; 6]; - - self.read_bytes(reg, &mut buf).map_err(Error::I2c)?; + /// Sets the gyroscope config + pub async fn set_gyr_config( + &mut self, + gyr_config: &GyrConfig, + delay: &mut D, + ) -> Result<(), Error> { + let prev_mode = self.mode; + self.set_mode(BNO055OperationMode::CONFIG_MODE, delay) + .await?; + self.set_page(BNO055RegisterPage::PAGE_1).await?; + let (bits0, bits1) = gyr_config.to_bits(); + self.write_u8(regs::BNO055_GYR_CONFIG_0, bits0).await?; + self.write_u8(regs::BNO055_GYR_CONFIG_1, bits1).await?; + self.set_mode(prev_mode, delay).await?; - let x = LittleEndian::read_i16(&buf[0..2]); - let y = LittleEndian::read_i16(&buf[2..4]); - let z = LittleEndian::read_i16(&buf[4..6]); + Ok(()) + } - Ok(mint::Vector3::from([x, y, z])) + /// Returns the current magnetometer config + pub async fn get_mag_config(&mut self) -> Result> { + self.set_page(BNO055RegisterPage::PAGE_1).await?; + let bits = self.read_u8(regs::BNO055_MAG_CONFIG).await?; + MagConfig::from_bits(bits).map_err(Error::MagConfig) } - /// Applies the given scaling to the vector of sensor data from the device. - fn scale_vec(raw: mint::Vector3, scaling: f32) -> mint::Vector3 { - mint::Vector3::from([ - raw.x as f32 * scaling, - raw.y as f32 * scaling, - raw.z as f32 * scaling, - ]) + /// Sets the magnetometer config + pub async fn set_mag_config( + &mut self, + mag_config: &MagConfig, + delay: &mut D, + ) -> Result<(), Error> { + let prev_mode = self.mode; + self.set_mode(BNO055OperationMode::CONFIG_MODE, delay) + .await?; + self.set_page(BNO055RegisterPage::PAGE_1).await?; + self.write_u8(regs::BNO055_MAG_CONFIG, mag_config.to_bits()) + .await?; + self.set_mode(prev_mode, delay).await?; + + Ok(()) } /// Returns linear acceleration vector in cm/s^2 units. - /// Available only in sensor fusion modes. - pub fn linear_acceleration_fixed(&mut self) -> Result, Error> { - if self.mode.is_fusion_enabled() { - self.set_page(BNO055RegisterPage::PAGE_0)?; - self.read_vec_raw(regs::BNO055_LIA_DATA_X_LSB) - } else { - Err(Error::InvalidMode) + pub async fn linear_acceleration_fixed(&mut self) -> Result, Error> { + if !self.mode.is_fusion_enabled() { + return Err(Error::InvalidMode); } + self.set_page(BNO055RegisterPage::PAGE_0).await?; + self.read_vec_raw(regs::BNO055_LIA_DATA_X_LSB).await } /// Returns linear acceleration vector in m/s^2 units. - /// Available only in sensor fusion modes. - pub fn linear_acceleration(&mut self) -> Result, Error> { - let linear_acceleration = self.linear_acceleration_fixed()?; - let scaling = 1f32 / 100f32; // 1 m/s^2 = 100 lsb - Ok(Self::scale_vec(linear_acceleration, scaling)) + pub async fn linear_acceleration(&mut self) -> Result, Error> { + let lia = self.linear_acceleration_fixed().await?; + Ok(Self::scale_vec(lia, ACCEL_SCALING)) } /// Returns gravity vector in cm/s^2 units. - /// Available only in sensor fusion modes. - pub fn gravity_fixed(&mut self) -> Result, Error> { - if self.mode.is_fusion_enabled() { - self.set_page(BNO055RegisterPage::PAGE_0)?; - self.read_vec_raw(regs::BNO055_GRV_DATA_X_LSB) - } else { - Err(Error::InvalidMode) + pub async fn gravity_fixed(&mut self) -> Result, Error> { + if !self.mode.is_fusion_enabled() { + return Err(Error::InvalidMode); } + self.set_page(BNO055RegisterPage::PAGE_0).await?; + self.read_vec_raw(regs::BNO055_GRV_DATA_X_LSB).await } /// Returns gravity vector in m/s^2 units. - /// Available only in sensor fusion modes. - pub fn gravity(&mut self) -> Result, Error> { - let gravity = self.gravity_fixed()?; - let scaling = 1f32 / 100f32; // 1 m/s^2 = 100 lsb - Ok(Self::scale_vec(gravity, scaling)) + pub async fn gravity(&mut self) -> Result, Error> { + let grv = self.gravity_fixed().await?; + Ok(Self::scale_vec(grv, ACCEL_SCALING)) + } + + /// Returns Acceleration and Gyroscope vectors in this order. + pub async fn dof6_fixed( + &mut self, + ) -> Result<(mint::Vector3, mint::Vector3), Error> { + if !self.mode.is_accel_enabled() || !self.mode.is_gyro_enabled() { + return Err(Error::InvalidMode); + } + self.set_page(BNO055RegisterPage::PAGE_0).await?; + let mut buf: [u8; 12] = [0; 12]; + self.read_bytes(regs::BNO055_ACC_DATA_X_LSB, &mut buf) + .await?; + let accel = mint::Vector3::from([ + LittleEndian::read_i16(&buf[0..2]), + LittleEndian::read_i16(&buf[2..4]), + LittleEndian::read_i16(&buf[4..6]), + ]); + let gyro = mint::Vector3::from([ + LittleEndian::read_i16(&buf[6..8]), + LittleEndian::read_i16(&buf[8..10]), + LittleEndian::read_i16(&buf[10..12]), + ]); + Ok((accel, gyro)) + } + + /// Returns Acceleration and Gyroscope vectors in this order. + pub async fn dof6(&mut self) -> Result<(mint::Vector3, mint::Vector3), Error> { + let (accel, gyro) = self.dof6_fixed().await?; + Ok(( + Self::scale_vec(accel, ACCEL_SCALING), + Self::scale_vec(gyro, GYRO_SCALING), + )) + } + + /// Returns Acceleration, Gyroscope and Magnetometer vectors in this order. + #[allow(clippy::type_complexity)] + pub async fn dof9_fixed( + &mut self, + ) -> Result<(mint::Vector3, mint::Vector3, mint::Vector3), Error> { + if !self.mode.is_accel_enabled() + || !self.mode.is_gyro_enabled() + || !self.mode.is_mag_enabled() + { + return Err(Error::InvalidMode); + } + self.set_page(BNO055RegisterPage::PAGE_0).await?; + let mut buf: [u8; 18] = [0; 18]; + self.read_bytes(regs::BNO055_ACC_DATA_X_LSB, &mut buf) + .await?; + let accel = mint::Vector3::from([ + LittleEndian::read_i16(&buf[0..2]), + LittleEndian::read_i16(&buf[2..4]), + LittleEndian::read_i16(&buf[4..6]), + ]); + let mag = mint::Vector3::from([ + LittleEndian::read_i16(&buf[6..8]), + LittleEndian::read_i16(&buf[8..10]), + LittleEndian::read_i16(&buf[10..12]), + ]); + let gyro = mint::Vector3::from([ + LittleEndian::read_i16(&buf[12..14]), + LittleEndian::read_i16(&buf[14..16]), + LittleEndian::read_i16(&buf[16..18]), + ]); + Ok((accel, mag, gyro)) + } + + /// Returns Acceleration, Gyroscope and Magnetometer vectors in this order. + #[allow(clippy::type_complexity)] + pub async fn dof9( + &mut self, + ) -> Result<(mint::Vector3, mint::Vector3, mint::Vector3), Error> { + let (accel, mag, gyro) = self.dof9_fixed().await?; + Ok(( + Self::scale_vec(accel, ACCEL_SCALING), + Self::scale_vec(mag, MAG_SCALING), + Self::scale_vec(gyro, GYRO_SCALING), + )) } /// Returns current accelerometer data in cm/s^2 units. - /// Available only in modes in which accelerometer is enabled. - pub fn accel_data_fixed(&mut self) -> Result, Error> { - if self.mode.is_accel_enabled() { - self.set_page(BNO055RegisterPage::PAGE_0)?; - self.read_vec_raw(regs::BNO055_ACC_DATA_X_LSB) - } else { - Err(Error::InvalidMode) + pub async fn accel_data_fixed(&mut self) -> Result, Error> { + if !self.mode.is_accel_enabled() { + return Err(Error::InvalidMode); } + self.set_page(BNO055RegisterPage::PAGE_0).await?; + self.read_vec_raw(regs::BNO055_ACC_DATA_X_LSB).await } /// Returns current accelerometer data in m/s^2 units. - /// Available only in modes in which accelerometer is enabled. - pub fn accel_data(&mut self) -> Result, Error> { - let a = self.accel_data_fixed()?; - let scaling = 1f32 / 100f32; // 1 m/s^2 = 100 lsb - Ok(Self::scale_vec(a, scaling)) + pub async fn accel_data(&mut self) -> Result, Error> { + let a = self.accel_data_fixed().await?; + Ok(Self::scale_vec(a, ACCEL_SCALING)) } /// Returns current gyroscope data in 1/16th deg/s units. - /// Available only in modes in which gyroscope is enabled. - pub fn gyro_data_fixed(&mut self) -> Result, Error> { - if self.mode.is_gyro_enabled() { - self.set_page(BNO055RegisterPage::PAGE_0)?; - self.read_vec_raw(regs::BNO055_GYR_DATA_X_LSB) - } else { - Err(Error::InvalidMode) + pub async fn gyro_data_fixed(&mut self) -> Result, Error> { + if !self.mode.is_gyro_enabled() { + return Err(Error::InvalidMode); } + self.set_page(BNO055RegisterPage::PAGE_0).await?; + self.read_vec_raw(regs::BNO055_GYR_DATA_X_LSB).await } /// Returns current gyroscope data in deg/s units. - /// Available only in modes in which gyroscope is enabled. - pub fn gyro_data(&mut self) -> Result, Error> { - let g = self.gyro_data_fixed()?; - let scaling = 1f32 / 16f32; // 1 deg/s = 16 lsb - Ok(Self::scale_vec(g, scaling)) + pub async fn gyro_data(&mut self) -> Result, Error> { + let g = self.gyro_data_fixed().await?; + Ok(Self::scale_vec(g, GYRO_SCALING)) } /// Returns current magnetometer data in 1/16th uT units. - /// Available only in modes in which magnetometer is enabled. - pub fn mag_data_fixed(&mut self) -> Result, Error> { - if self.mode.is_mag_enabled() { - self.set_page(BNO055RegisterPage::PAGE_0)?; - self.read_vec_raw(regs::BNO055_MAG_DATA_X_LSB) - } else { - Err(Error::InvalidMode) + pub async fn mag_data_fixed(&mut self) -> Result, Error> { + if !self.mode.is_mag_enabled() { + return Err(Error::InvalidMode); } + self.set_page(BNO055RegisterPage::PAGE_0).await?; + self.read_vec_raw(regs::BNO055_MAG_DATA_X_LSB).await } /// Returns current magnetometer data in uT units. - /// Available only in modes in which magnetometer is enabled. - pub fn mag_data(&mut self) -> Result, Error> { - let m = self.mag_data_fixed()?; - let scaling = 1f32 / 16f32; // 1 uT = 16 lsb - Ok(Self::scale_vec(m, scaling)) + pub async fn mag_data(&mut self) -> Result, Error> { + let m = self.mag_data_fixed().await?; + Ok(Self::scale_vec(m, MAG_SCALING)) } /// Returns current temperature of the chip (in degrees Celsius). - pub fn temperature(&mut self) -> Result> { - self.set_page(BNO055RegisterPage::PAGE_0)?; - - // Read temperature signed byte - let temp = self.read_u8(regs::BNO055_TEMP).map_err(Error::I2c)? as i8; + pub async fn temperature(&mut self) -> Result> { + self.set_page(BNO055RegisterPage::PAGE_0).await?; + let temp = self.read_u8(regs::BNO055_TEMP).await? as i8; Ok(temp) } + /// Read which interrupts are currently triggered/active + pub async fn interrupts_triggered(&mut self) -> Result> { + self.set_page(BNO055RegisterPage::PAGE_0).await?; + let bits = self.read_u8(regs::BNO055_INT_STA).await?; + Ok(BNO055Interrupt::from_u8(bits).unwrap_or_default()) + // self.read_flags(BNO055RegisterPage::PAGE_0, regs::BNO055_INT_STA) + // .await + } + + /// Resets the interrupts register and the INT pin. + /// + pub async fn clear_interrupts(&mut self) -> Result<(), Error> { + self.set_page(BNO055RegisterPage::PAGE_0).await?; + // We need to fetch the SYS_TRIG first as the external clock bit might be set. + let sys_trig = self.read_u8(regs::BNO055_SYS_TRIGGER).await?; + self.write_u8( + regs::BNO055_SYS_TRIGGER, + sys_trig | BNO055SystemTrigger::RST_INT.bits(), + ) + .await + // self.write_flags( + // BNO055RegisterPage::PAGE_0, + // regs::BNO055_SYS_TRIGGER, + // BNO055SystemTrigger::RST_INT, + // ) + // .await + } + + /// Sets which interrupts are enabled, overrides all current interrupts + pub async fn set_interrupts_enabled( + &mut self, + interrupts: BNO055Interrupt, + delay: &mut D, + ) -> Result<(), Error> { + let prev_mode = self.mode; + self.set_mode(BNO055OperationMode::CONFIG_MODE, delay) + .await?; + + self.set_page(BNO055RegisterPage::PAGE_1).await?; + self.write_u8(regs::BNO055_INT_EN, interrupts.bits()) + .await?; + // self.write_flags(BNO055RegisterPage::PAGE_1, regs::BNO055_INT_EN, interrupts) + // .await?; + + self.set_mode(prev_mode, delay).await + } + + /// Returns currently enabled interrupts + pub async fn interrupts_enabled(&mut self) -> Result> { + self.set_page(BNO055RegisterPage::PAGE_1).await?; + let bits = self.read_u8(regs::BNO055_INT_EN).await?; + Ok(BNO055Interrupt::from_u8(bits).unwrap_or_default()) + // self.read_flags(BNO055RegisterPage::PAGE_1, regs::BNO055_INT_EN) + // .await + } + + /// Sets interrupts mask + pub async fn set_interrupts_mask( + &mut self, + mask: BNO055Interrupt, + delay: &mut D, + ) -> Result<(), Error> { + let prev_mode = self.mode; + self.set_mode(BNO055OperationMode::CONFIG_MODE, delay) + .await?; + + self.set_page(BNO055RegisterPage::PAGE_1).await?; + self.write_u8(regs::BNO055_INT_MSK, mask.bits()).await?; + // self.write_flags(BNO055RegisterPage::PAGE_1, regs::BNO055_INT_MSK, mask) + // .await + + self.set_mode(prev_mode, delay).await + } + + /// Returns the current interrupts mask + pub async fn interrupts_mask(&mut self) -> Result> { + self.set_page(BNO055RegisterPage::PAGE_1).await?; + let bits = self.read_u8(regs::BNO055_INT_MSK).await?; + Ok(BNO055Interrupt::from_u8(bits).unwrap_or_default()) + // self.read_flags(BNO055RegisterPage::PAGE_1, regs::BNO055_INT_MSK) + // .await + } + + /// Sets accelerometer interrupt settings + pub async fn set_acc_interrupt_settings( + &mut self, + settings: BNO055AccIntSettings, + delay: &mut D, + ) -> Result<(), Error> { + self.set_config_from( + BNO055RegisterPage::PAGE_1, + regs::BNO055_ACC_INT_SETTING, + settings, + delay, + ) + .await + } + + /// Returns current accelerometer interrupt settings + pub async fn acc_interrupt_settings(&mut self) -> Result> { + self.read_u8_into(BNO055RegisterPage::PAGE_1, regs::BNO055_ACC_INT_SETTING) + .await? + .ok_or(Error::AccConfig(acc_config::Error::BadAccIntSettings)) + } + + /// Sets accelerometer any motion interrupt threshold setting + pub async fn set_acc_am_threshold( + &mut self, + mult: u8, + delay: &mut D, + ) -> Result<(), Error> { + self.set_config_from( + BNO055RegisterPage::PAGE_1, + regs::BNO055_ACC_AM_THRES, + mult, + delay, + ) + .await + } + + /// Returns current accelerometer any motion interrupt threshold setting + pub async fn acc_am_threshold(&mut self) -> Result> { + self.read_u8_into::(BNO055RegisterPage::PAGE_1, regs::BNO055_ACC_AM_THRES) + .await? + .ok_or(Error::AccConfig(acc_config::Error::BadAccAmThreshold)) + } + + /// Sets accelerometer High-G interrupt duration setting + pub async fn set_acc_hg_duration( + &mut self, + dur: u8, + delay: &mut D, + ) -> Result<(), Error> { + self.set_config_from( + BNO055RegisterPage::PAGE_1, + regs::BNO055_ACC_HG_DURATION, + dur, + delay, + ) + .await + } + + /// Returns current accelerometer High-G interrupt duration setting + pub async fn acc_hg_duration(&mut self) -> Result> { + self.read_u8_into::(BNO055RegisterPage::PAGE_1, regs::BNO055_ACC_HG_DURATION) + .await? + .ok_or(Error::AccConfig(acc_config::Error::BadAccHgDuration)) + } + + /// Sets accelerometer High-G interrupt threshold setting + pub async fn set_acc_hg_threshold( + &mut self, + mult: u8, + delay: &mut D, + ) -> Result<(), Error> { + self.set_config_from( + BNO055RegisterPage::PAGE_1, + regs::BNO055_ACC_HG_THRES, + mult, + delay, + ) + .await + } + + /// Returns current accelerometer High-G interrupt threshold setting + pub async fn acc_hg_threshold(&mut self) -> Result> { + self.read_u8_into::(BNO055RegisterPage::PAGE_1, regs::BNO055_ACC_HG_THRES) + .await? + .ok_or(Error::AccConfig(acc_config::Error::BadAccHgThreshold)) + } + + /// Sets accelerometer no/slow-motion interrupt threshold setting + pub async fn set_acc_nm_threshold( + &mut self, + mult: u8, + delay: &mut D, + ) -> Result<(), Error> { + self.set_config_from( + BNO055RegisterPage::PAGE_1, + regs::BNO055_ACC_NM_THRES, + mult, + delay, + ) + .await + } + + /// Returns current accelerometer no/slow-motion interrupt threshold setting + pub async fn acc_nm_threshold(&mut self) -> Result> { + self.read_u8_into::(BNO055RegisterPage::PAGE_1, regs::BNO055_ACC_NM_THRES) + .await? + .ok_or(Error::AccConfig(acc_config::Error::BadAccNmThreshold)) + } + + /// Sets accelerometer no/slow-motion interrupt settings + pub async fn set_acc_nm_settings( + &mut self, + settings: BNO055AccNmSettings, + delay: &mut D, + ) -> Result<(), Error> { + self.set_config_from( + BNO055RegisterPage::PAGE_1, + regs::BNO055_ACC_NM_SET, + settings, + delay, + ) + .await + } + + /// Returns current accelerometer no/slow-motion interrupt settings + pub async fn acc_nm_settings(&mut self) -> Result> { + self.read_u8_into(BNO055RegisterPage::PAGE_1, regs::BNO055_ACC_NM_SET) + .await? + .ok_or(Error::AccConfig(acc_config::Error::BadAccNmSettings)) + } + + /// Sets gyroscope interrupt settings + pub async fn set_gyr_interrupt_settings( + &mut self, + settings: BNO055GyrIntSettings, + delay: &mut D, + ) -> Result<(), Error> { + self.write_config_flags( + BNO055RegisterPage::PAGE_1, + regs::BNO055_GYR_INT_SETTING, + settings, + delay, + ) + .await + } + + /// Returns the current gyroscope interrupt settings + pub async fn gyr_interrupt_settings(&mut self) -> Result> { + self.read_flags(BNO055RegisterPage::PAGE_1, regs::BNO055_GYR_INT_SETTING) + .await + } + + /// Sets gyroscope high-rate interrupt settings for x-axis + pub async fn set_gyr_hr_x_settings( + &mut self, + settings: BNO055GyrHrSettings, + delay: &mut D, + ) -> Result<(), Error> { + self.set_config_from( + BNO055RegisterPage::PAGE_1, + regs::BNO055_GYR_HR_X_SET, + settings, + delay, + ) + .await + } + + /// Returns current gyroscope high-rate interrupt settings for x-axis + pub async fn gyr_hr_x_settings(&mut self) -> Result> { + self.read_u8_into(BNO055RegisterPage::PAGE_1, regs::BNO055_GYR_HR_X_SET) + .await? + .ok_or(Error::GyroConfig(gyr_config::Error::BadGyrHrSettings)) + } + + /// Sets gyroscope high-rate interrupt duration for x-axis + pub async fn set_gyr_dur_x( + &mut self, + duration: u8, + delay: &mut D, + ) -> Result<(), Error> { + self.set_config_from( + BNO055RegisterPage::PAGE_1, + regs::BNO055_GYR_DUR_X, + duration, + delay, + ) + .await + } + + /// Returns current gyroscope high-rate interrupt settings for x-axis + pub async fn gyr_dur_x(&mut self) -> Result> { + self.read_u8_into::(BNO055RegisterPage::PAGE_1, regs::BNO055_GYR_DUR_X) + .await? + .ok_or(Error::GyroConfig(gyr_config::Error::BadGyrDurX)) + } + + /// Sets gyroscope high-rate interrupt settings for y-axis + pub async fn set_gyr_hr_y_settings( + &mut self, + settings: BNO055GyrHrSettings, + delay: &mut D, + ) -> Result<(), Error> { + self.set_config_from( + BNO055RegisterPage::PAGE_1, + regs::BNO055_GYR_HR_Y_SET, + settings, + delay, + ) + .await + } + + /// Returns current gyroscope high-rate interrupt settings for y-axis + pub async fn gyr_hr_y_settings(&mut self) -> Result> { + self.read_u8_into(BNO055RegisterPage::PAGE_1, regs::BNO055_GYR_HR_Y_SET) + .await? + .ok_or(Error::GyroConfig(gyr_config::Error::BadGyrHrYSettings)) + } + + /// Sets gyroscope high-rate interrupt duration for y-axis + pub async fn set_gyr_dur_y( + &mut self, + duration: u8, + delay: &mut D, + ) -> Result<(), Error> { + self.set_config_from( + BNO055RegisterPage::PAGE_1, + regs::BNO055_GYR_DUR_Y, + duration, + delay, + ) + .await + } + + /// Returns current gyroscope high-rate interrupt settings for y-axis + pub async fn gyr_dur_y(&mut self) -> Result> { + self.read_u8_into::(BNO055RegisterPage::PAGE_1, regs::BNO055_GYR_DUR_Y) + .await? + .ok_or(Error::GyroConfig(gyr_config::Error::BadGyrDurY)) + } + + /// Sets gyroscope high-rate interrupt settings for z-axis + pub async fn set_gyr_hr_z_settings( + &mut self, + settings: BNO055GyrHrSettings, + delay: &mut D, + ) -> Result<(), Error> { + self.set_config_from( + BNO055RegisterPage::PAGE_1, + regs::BNO055_GYR_HR_Z_SET, + settings, + delay, + ) + .await + } + + /// Returns current gyroscope high-rate interrupt settings for z-axis + pub async fn gyr_hr_z_settings(&mut self) -> Result> { + self.read_u8_into(BNO055RegisterPage::PAGE_1, regs::BNO055_GYR_HR_Z_SET) + .await? + .ok_or(Error::GyroConfig(gyr_config::Error::BadGyrHrZSettings)) + } + + /// Sets gyroscope high-rate interrupt duration for z-axis + pub async fn set_gyr_dur_z( + &mut self, + duration: u8, + delay: &mut D, + ) -> Result<(), Error> { + self.set_config_from( + BNO055RegisterPage::PAGE_1, + regs::BNO055_GYR_DUR_Z, + duration, + delay, + ) + .await + } + + /// Returns current gyroscope high-rate interrupt settings for z-axis + pub async fn gyr_dur_z(&mut self) -> Result> { + self.read_u8_into::(BNO055RegisterPage::PAGE_1, regs::BNO055_GYR_DUR_Z) + .await? + .ok_or(Error::GyroConfig(gyr_config::Error::BadGyrDurZ)) + } + + /// Sets gyroscope any-motion interrupt threshold + pub async fn set_gyr_am_threshold( + &mut self, + mult: u8, + delay: &mut D, + ) -> Result<(), Error> { + let mult = mult & BIT_7_RESERVED_MASK; // Ensure reserved bit is 0 + self.set_config_from( + BNO055RegisterPage::PAGE_1, + regs::BNO055_GYR_AM_THRES, + mult, + delay, + ) + .await + } + + /// Returns current gyroscope high-rate interrupt settings for z-axis + pub async fn gyr_am_threshold(&mut self) -> Result> { + let mult = self + .read_u8_into::(BNO055RegisterPage::PAGE_1, regs::BNO055_GYR_AM_THRES) + .await? + .ok_or(Error::GyroConfig(gyr_config::Error::BadGyrAmThreshold))?; + Ok(mult & BIT_7_RESERVED_MASK) + } + + /// Sets gyroscope any-motion interrupt settings + pub async fn set_gyr_am_settings( + &mut self, + settings: BNO055GyrAmSettings, + delay: &mut D, + ) -> Result<(), Error> { + self.set_config_from( + BNO055RegisterPage::PAGE_1, + regs::BNO055_GYR_AM_SET, + settings, + delay, + ) + .await + } + + /// Returns current gyroscope any-motion interrupt settings + pub async fn gyr_am_settings(&mut self) -> Result> { + self.read_u8_into(BNO055RegisterPage::PAGE_1, regs::BNO055_GYR_AM_SET) + .await? + .ok_or(Error::GyroConfig(gyr_config::Error::BadGyrAmSettings)) + } + + // ------------------ + // Private helper functions + // ------------------ + #[inline(always)] fn i2c_addr(&self) -> u8 { - if !self.use_default_addr { - regs::BNO055_ALTERNATE_ADDR - } else { + if self.use_default_addr { regs::BNO055_DEFAULT_ADDR + } else { + regs::BNO055_ALTERNATE_ADDR } } - fn read_u8(&mut self, reg: u8) -> Result { - let mut byte: [u8; 1] = [0; 1]; - - match self.i2c.write_read(self.i2c_addr(), &[reg], &mut byte) { - Ok(_) => Ok(byte[0]), - Err(e) => Err(e), - } + /// Sets current register map page. + async fn set_page(&mut self, page: BNO055RegisterPage) -> Result<(), Error> { + self.write_u8(regs::BNO055_PAGE_ID, page.bits()).await } - fn read_bytes(&mut self, reg: u8, buf: &mut [u8]) -> Result<(), E> { - self.i2c.write_read(self.i2c_addr(), &[reg], buf) + /// Reads a vector of sensor data from the device. + async fn read_vec_raw(&mut self, reg: u8) -> Result, Error> { + let mut buf: [u8; 6] = [0; 6]; + self.read_bytes(reg, &mut buf).await?; + let x = LittleEndian::read_i16(&buf[0..2]); + let y = LittleEndian::read_i16(&buf[2..4]); + let z = LittleEndian::read_i16(&buf[4..6]); + Ok(mint::Vector3::from([x, y, z])) } - fn write_u8(&mut self, reg: u8, value: u8) -> Result<(), E> { - self.i2c.write(self.i2c_addr(), &[reg, value])?; + /// Applies the given scaling to the vector of sensor data from the device. + fn scale_vec(raw: mint::Vector3, scaling: f32) -> mint::Vector3 { + mint::Vector3::from([ + raw.x as f32 * scaling, + raw.y as f32 * scaling, + raw.z as f32 * scaling, + ]) + } + /// Helper to set a value from a type that can be converted to u8 + async fn set_u8_from + Send>( + &mut self, + page: BNO055RegisterPage, + reg: u8, + x: T, + ) -> Result<(), Error> { + self.set_page(page).await?; + self.write_u8(reg, x.into()).await?; Ok(()) } + + /// Helper to set a configuration value, which requires switching to CONFIG_MODE + async fn set_config_from + Send, D: DelayNs>( + &mut self, + page: BNO055RegisterPage, + reg: u8, + x: T, + delay: &mut D, + ) -> Result<(), Error> { + let prev = self.mode; + self.set_mode(BNO055OperationMode::CONFIG_MODE, delay) + .await?; + let res = self.set_u8_from(page, reg, x).await; + self.set_mode(prev, delay).await?; + res + } + + /// Helper to read a u8 value and convert it to a target type + async fn read_u8_into( + &mut self, + page: BNO055RegisterPage, + reg: u8, + ) -> Result, Error> { + self.set_page(page).await?; + let regval = self.read_u8(reg).await?; + Ok(T::from_u8(regval)) + } + + /// Helper to write bitflags to a register + async fn write_flags( + &mut self, + page: BNO055RegisterPage, + reg: u8, + flags: F, + ) -> Result<(), Error> + where + F: bitflags::Flags + Send, + { + self.set_page(page).await?; + self.write_u8(reg, flags.bits()).await + } + + /// Helper to write configuration bitflags, which requires switching to CONFIG_MODE + async fn write_config_flags( + &mut self, + page: BNO055RegisterPage, + reg: u8, + flags: F, + delay: &mut D, + ) -> Result<(), Error> + where + F: bitflags::Flags + Send, + D: DelayNs, + { + let prev = self.mode; + self.set_mode(BNO055OperationMode::CONFIG_MODE, delay) + .await?; + let res = self.write_flags(page, reg, flags).await; + self.set_mode(prev, delay).await?; + res + } + + /// Helper to read bitflags from a register + async fn read_flags(&mut self, page: BNO055RegisterPage, reg: u8) -> Result> + where + F: bitflags::Flags, + { + self.set_page(page).await?; + let bits = self.read_u8(reg).await?; + Ok(F::from_bits_truncate(bits)) + } + + /// Low-level I2C read of a single u8 + async fn read_u8(&mut self, reg: u8) -> Result> { + let mut byte: [u8; 1] = [0; 1]; + (self + .i2c + .write_read(self.i2c_addr(), &[reg], &mut byte) + .await) + .map_err(Error::I2c)?; + Ok(byte[0]) + } + + /// Low-level I2C read of multiple bytes + async fn read_bytes(&mut self, reg: u8, buf: &mut [u8]) -> Result<(), Error> { + (self.i2c.write_read(self.i2c_addr(), &[reg], buf).await).map_err(Error::I2c) + } + + /// Low-level I2C write of a single u8 + async fn write_u8(&mut self, reg: u8, value: u8) -> Result<(), Error> { + (self.i2c.write(self.i2c_addr(), &[reg, value]).await).map_err(Error::I2c) + } + + /// Low-level I2C write of multiple bytes + async fn write_bytes(&mut self, reg: u8, values: &[u8]) -> Result<(), Error> { + let mut buffer = [0u8; BNO055_CALIB_SIZE + 1]; + buffer[0] = reg; + buffer[1..values.len() + 1].copy_from_slice(values); + (self + .i2c + .write(self.i2c_addr(), &buffer[..=values.len()]) + .await) + .map_err(Error::I2c) + } } bitflags! { - #[cfg_attr(not(feature = "defmt-03"), derive(Debug, Clone, Copy, PartialEq, Eq))] + #[derive(Debug, Clone, Copy, PartialEq, Eq)] pub struct BNO055AxisConfig: u8 { const AXIS_AS_X = 0b00; const AXIS_AS_Y = 0b01; @@ -672,6 +1241,22 @@ bitflags! { } } +#[cfg(feature = "defmt")] +impl defmt::Format for BNO055AxisConfig { + fn format(&self, f: defmt::Formatter) { + defmt::write!( + f, + "BNO055AxisConfig({})", + match self.bits() { + 0b00 => "X", + 0b01 => "Y", + 0b10 => "Z", + _ => "Unknown", + } + ) + } +} + #[allow(clippy::misnamed_getters)] impl AxisRemap { pub fn x(&self) -> BNO055AxisConfig { @@ -688,6 +1273,7 @@ impl AxisRemap { } #[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] pub struct AxisRemap { x: BNO055AxisConfig, y: BNO055AxisConfig, @@ -778,7 +1364,7 @@ impl AxisRemapBuilder { } bitflags! { - #[cfg_attr(not(feature = "defmt-03"), derive(Debug, Clone, Copy, PartialEq, Eq))] + #[derive(Debug, Clone, Copy, PartialEq, Eq)] pub struct BNO055AxisSign: u8 { const X_NEGATIVE = 0b100; const Y_NEGATIVE = 0b010; @@ -786,22 +1372,70 @@ bitflags! { } } +#[cfg(feature = "defmt")] +impl defmt::Format for BNO055AxisSign { + fn format(&self, f: defmt::Formatter) { + defmt::write!(f, "BNO055AxisSign("); + if self.is_empty() { + defmt::write!(f, "Positive"); + } else { + if self.contains(BNO055AxisSign::X_NEGATIVE) { + defmt::write!(f, "X_NEGATIVE "); + } + if self.contains(BNO055AxisSign::Y_NEGATIVE) { + defmt::write!(f, "Y_NEGATIVE "); + } + if self.contains(BNO055AxisSign::Z_NEGATIVE) { + defmt::write!(f, "Z_NEGATIVE "); + } + } + defmt::write!(f, ")"); + } +} + bitflags! { - #[cfg_attr(not(feature = "defmt-03"), derive(Debug, Clone, Copy, PartialEq, Eq))] + #[derive(Debug, Clone, Copy, PartialEq, Eq)] + /// SYS_STATUS 0x39 pub struct BNO055SystemStatusCode: u8 { + /// 0 System idle const SYSTEM_IDLE = 0; + /// 1 System Error const SYSTEM_ERROR = 1; + /// 2 Initializing peripherals const INIT_PERIPHERALS = 2; + /// 3 System Initialization const SYSTEM_INIT = 3; + /// 4 Executing selftest const EXECUTING = 4; + /// 5 Sensor fusion algorithm running const RUNNING = 5; + /// 6 System running without fusion algorithm const RUNNING_WITHOUT_FUSION = 6; } } +#[cfg(feature = "defmt")] +impl defmt::Format for BNO055SystemStatusCode { + fn format(&self, f: defmt::Formatter) { + defmt::write!( + f, + "BNO055SystemStatusCode({})", + match self.bits() { + 0 => "SystemIdle", + 1 => "SystemError", + 2 => "InitPeripherals", + 3 => "SystemInit", + 4 => "Executing", + 5 => "Running", + 6 => "RunningWithoutFusion", + _ => "Unknown", + } + ) + } +} + bitflags! { - /// Possible BNO055 errors. - #[cfg_attr(not(feature = "defmt-03"), derive(Debug, Clone, Copy, PartialEq, Eq))] + #[derive(Debug, Clone, Copy, PartialEq, Eq)] pub struct BNO055SystemErrorCode: u8 { const NONE = 0; const PERIPHERAL_INIT = 1; @@ -817,27 +1451,73 @@ bitflags! { } } +#[cfg(feature = "defmt")] +impl defmt::Format for BNO055SystemErrorCode { + fn format(&self, f: defmt::Formatter) { + defmt::write!( + f, + "BNO055SystemErrorCode({})", + match self.bits() { + 0 => "None", + 1 => "PeripheralInit", + 2 => "SystemInit", + 3 => "SelfTest", + 4 => "RegisterMapValue", + 5 => "RegisterMapAddress", + 6 => "RegisterMapWrite", + 7 => "LowPowerModeNotAvail", + 8 => "AccelPowerModeNotAvail", + 9 => "FusionAlgoConfig", + 10 => "SensorConfig", + _ => "Unknown", + } + ) + } +} + bitflags! { - /// BNO055 self-test status bit flags. - #[cfg_attr(not(feature = "defmt-03"), derive(Debug, Clone, Copy, PartialEq, Eq))] + #[derive(Debug, Clone, Copy, PartialEq, Eq)] pub struct BNO055SelfTestStatus: u8 { const ACC_OK = 0b0001; const MAG_OK = 0b0010; const GYR_OK = 0b0100; - const SYS_OK = 0b1000; + const MCU_OK = 0b1000; // Corrected from SYS_OK to match datasheet + } +} + +#[cfg(feature = "defmt")] +impl defmt::Format for BNO055SelfTestStatus { + fn format(&self, f: defmt::Formatter) { + defmt::write!(f, "BNO055SelfTestStatus("); + if self.is_empty() { + defmt::write!(f, "None"); + } else { + if self.contains(BNO055SelfTestStatus::ACC_OK) { + defmt::write!(f, "ACC_OK"); + } + if self.contains(BNO055SelfTestStatus::MAG_OK) { + defmt::write!(f, "MAG_OK"); + } + if self.contains(BNO055SelfTestStatus::GYR_OK) { + defmt::write!(f, "GYR_OK"); + } + if self.contains(BNO055SelfTestStatus::MCU_OK) { + defmt::write!(f, "MCU_OK"); + } + } + defmt::write!(f, ")"); } } #[derive(Debug, Clone, PartialEq, Eq)] -#[cfg_attr(feature = "defmt-03", derive(defmt::Format))] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] pub struct BNO055SystemStatus { - status: BNO055SystemStatusCode, - selftest: Option, - error: BNO055SystemErrorCode, + pub status: BNO055SystemStatusCode, + pub selftest: Option, + pub error: BNO055SystemErrorCode, } #[derive(Debug, Clone, Copy, PartialEq, Eq)] -#[cfg_attr(feature = "defmt-03", derive(defmt::Format))] pub struct BNO055Revision { pub software: u16, pub bootloader: u8, @@ -846,9 +1526,21 @@ pub struct BNO055Revision { pub gyroscope: u8, } +#[cfg(feature = "defmt")] +impl defmt::Format for BNO055Revision { + fn format(&self, f: defmt::Formatter) { + let [major, minor] = self.software.to_be_bytes(); + defmt::write!( + f, + "BNO055Revision {{ software: {=u8}.{=u8}, bootloader: {=u8}, accelerometer: {=u8}, magnetometer: {=u8}, gyroscope: {=u8} }}", + major, minor, self.bootloader, self.accelerometer, self.magnetometer, self.gyroscope + ) + } +} + #[derive(Debug, Clone, Copy, PartialEq, Eq)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -#[cfg_attr(feature = "defmt-03", derive(defmt::Format))] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] #[repr(C)] pub struct BNO055Calibration { pub acc_offset_x_lsb: u8, @@ -887,17 +1579,12 @@ impl BNO055Calibration { } pub fn as_bytes(&self) -> &[u8] { - unsafe { - core::slice::from_raw_parts( - (self as *const _) as *const u8, - ::core::mem::size_of::(), - ) - } + unsafe { core::slice::from_raw_parts((self as *const _) as *const u8, BNO055_CALIB_SIZE) } } } #[derive(Debug, Clone, Copy, PartialEq, Eq)] -#[cfg_attr(feature = "defmt-03", derive(defmt::Format))] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] pub struct BNO055CalibrationStatus { pub sys: u8, pub gyr: u8, @@ -906,17 +1593,30 @@ pub struct BNO055CalibrationStatus { } bitflags! { - /// Possible BNO055 register map pages. - #[cfg_attr(not(feature = "defmt-03"), derive(Debug, Clone, Copy, PartialEq, Eq))] + #[derive(Debug, Clone, Copy, PartialEq, Eq)] pub struct BNO055RegisterPage: u8 { const PAGE_0 = 0; const PAGE_1 = 1; } } +#[cfg(feature = "defmt")] +impl defmt::Format for BNO055RegisterPage { + fn format(&self, f: defmt::Formatter) { + defmt::write!( + f, + "BNO055RegisterPage({})", + match self.bits() { + 0 => "Page0", + 1 => "Page1", + _ => "Unknown", + } + ) + } +} + bitflags! { - /// Possible BNO055 power modes. - #[cfg_attr(not(feature = "defmt-03"), derive(Debug, Clone, Copy, PartialEq, Eq))] + #[derive(Default, Debug, Clone, Copy, PartialEq, Eq)] pub struct BNO055PowerMode: u8 { const NORMAL = 0b00; const LOW_POWER = 0b01; @@ -924,9 +1624,24 @@ bitflags! { } } +#[cfg(feature = "defmt")] +impl defmt::Format for BNO055PowerMode { + fn format(&self, f: defmt::Formatter) { + defmt::write!( + f, + "BNO055PowerMode({})", + match self.bits() { + 0b00 => "Normal", + 0b01 => "LowPower", + 0b10 => "Suspend", + _ => "Unknown", + } + ) + } +} + bitflags! { - /// Possible BNO055 operation modes. - #[cfg_attr(not(feature = "defmt-03"), derive(Debug, Clone, Copy, PartialEq, Eq))] + #[derive(Debug, Clone, Copy, PartialEq, Eq)] pub struct BNO055OperationMode: u8 { const CONFIG_MODE = 0b0000; const ACC_ONLY = 0b0001; @@ -944,6 +1659,38 @@ bitflags! { } } +impl Default for BNO055OperationMode { + fn default() -> Self { + Self::CONFIG_MODE + } +} + +#[cfg(feature = "defmt")] +impl defmt::Format for BNO055OperationMode { + fn format(&self, f: defmt::Formatter) { + defmt::write!( + f, + "BNO055OperationMode({})", + match self.bits() { + 0b0000 => "ConfigMode", + 0b0001 => "AccOnly", + 0b0010 => "MagOnly", + 0b0011 => "GyroOnly", + 0b0100 => "AccMag", + 0b0101 => "AccGyro", + 0b0110 => "MagGyro", + 0b0111 => "Amg", + 0b1000 => "Imu", + 0b1001 => "Compass", + 0b1010 => "M4g", + 0b1011 => "NdofFmcOff", + 0b1100 => "Ndof", + _ => "Unknown", + } + ) + } +} + impl BNO055OperationMode { fn is_fusion_enabled(&self) -> bool { matches!( @@ -994,3 +1741,151 @@ impl BNO055OperationMode { ) } } + +bitflags! { + #[derive(Default, Debug, Clone, Copy, PartialEq, Eq)] + pub struct BNO055Interrupt: u8 { + const ACC_NM = 0b10000000; + const ACC_AM = 0b01000000; + const ACC_HIGH_G = 0b00100000; + const GYR_DRDY = 0b00010000; + const GYR_HIGH_RATE = 0b00001000; + const GYRO_AM = 0b00000100; + const MAG_DRDY = 0b00000010; + const ACC_BSX_DRDY = 0b00000001; + } +} + +impl FromPrimitive for BNO055Interrupt { + /// Converts an i64 to a `BNO055Interrupt` if it's a valid bit pattern. + fn from_i64(n: i64) -> Option { + u8::from_i64(n).and_then(Self::from_bits) + } + + /// Converts a u64 to a `BNO055Interrupt` if it's a valid bit pattern. + fn from_u64(n: u64) -> Option { + u8::from_u64(n).and_then(Self::from_bits) + } + + /// Converts a u8 to a `BNO055Interrupt` if it's a valid bit pattern. + fn from_u8(n: u8) -> Option { + Self::from_bits(n) + } +} + +#[cfg(feature = "defmt")] +impl defmt::Format for BNO055Interrupt { + fn format(&self, f: defmt::Formatter) { + defmt::write!(f, "BNO055Interrupt("); + if self.is_empty() { + defmt::write!(f, "None"); + } else { + if self.contains(BNO055Interrupt::ACC_NM) { + defmt::write!(f, "ACC_NM "); + } + if self.contains(BNO055Interrupt::ACC_AM) { + defmt::write!(f, "ACC_AM "); + } + if self.contains(BNO055Interrupt::ACC_HIGH_G) { + defmt::write!(f, "ACC_HIGH_G "); + } + if self.contains(BNO055Interrupt::GYR_DRDY) { + defmt::write!(f, "GYR_DRDY "); + } + if self.contains(BNO055Interrupt::GYR_HIGH_RATE) { + defmt::write!(f, "GYR_HIGH_RATE "); + } + if self.contains(BNO055Interrupt::GYRO_AM) { + defmt::write!(f, "GYRO_AM "); + } + if self.contains(BNO055Interrupt::MAG_DRDY) { + defmt::write!(f, "MAG_DRDY "); + } + if self.contains(BNO055Interrupt::ACC_BSX_DRDY) { + defmt::write!(f, "ACC_BSX_DRDY "); + } + } + defmt::write!(f, ")"); + } +} + +bitflags! { + #[derive(Debug, Clone, Copy, PartialEq, Eq)] + pub struct BNO055SystemTrigger: u8 { + /// Select External Clock + const EXT_CLK_SEL = 0b1000_0000; + /// Clear interrupts command + const RST_INT = 0b0100_0000; + /// Reset command + const RST_SYS = 0b0010_0000; + /// Self-test command + const SELF_TEST = 0b0000_0001; + } +} + +impl FromPrimitive for BNO055SystemTrigger { + /// Converts an i64 to a `BNO055SystemTrigger` if it's a valid bit pattern. + fn from_i64(n: i64) -> Option { + u8::from_i64(n).and_then(Self::from_bits) + } + + /// Converts a u64 to a `BNO055SystemTrigger` if it's a valid bit pattern. + fn from_u64(n: u64) -> Option { + u8::from_u64(n).and_then(Self::from_bits) + } + + /// Converts a u8 to a `BNO055SystemTrigger` if it's a valid bit pattern. + fn from_u8(n: u8) -> Option { + Self::from_bits(n) + } +} + +#[cfg(feature = "defmt")] +impl defmt::Format for BNO055SystemTrigger { + fn format(&self, f: defmt::Formatter) { + defmt::write!(f, "BNO055SystemTrigger("); + if self.is_empty() { + defmt::write!(f, "None"); + } else { + if self.contains(BNO055SystemTrigger::EXT_CLK_SEL) { + defmt::write!(f, "EXT_CLK_SEL "); + } + if self.contains(BNO055SystemTrigger::RST_INT) { + defmt::write!(f, "RST_INT "); + } + if self.contains(BNO055SystemTrigger::RST_SYS) { + defmt::write!(f, "RST_SYS "); + } + if self.contains(BNO055SystemTrigger::SELF_TEST) { + defmt::write!(f, "SELF_TEST "); + } + } + defmt::write!(f, ")"); + } +} + +#[cfg(test)] +mod tests { + use crate::BNO055Interrupt; + + #[test] + fn test_interrupts() { + assert!(BNO055Interrupt::ACC_NM.contains(BNO055Interrupt::ACC_NM)); + assert!((BNO055Interrupt::ACC_NM | BNO055Interrupt::ACC_HIGH_G) + .contains(BNO055Interrupt::ACC_NM)); + assert!((BNO055Interrupt::all()).contains(BNO055Interrupt::ACC_NM)); + assert!((BNO055Interrupt::all()).contains(BNO055Interrupt::ACC_AM)); + assert!((BNO055Interrupt::all()).contains(BNO055Interrupt::ACC_HIGH_G)); + assert!((BNO055Interrupt::all()).contains(BNO055Interrupt::GYR_DRDY)); + assert!((BNO055Interrupt::all()).contains(BNO055Interrupt::GYR_HIGH_RATE)); + assert!((BNO055Interrupt::all()).contains(BNO055Interrupt::GYRO_AM)); + assert!((BNO055Interrupt::all()).contains(BNO055Interrupt::MAG_DRDY)); + assert!((BNO055Interrupt::all()).contains(BNO055Interrupt::ACC_BSX_DRDY)); + + assert!( + BNO055Interrupt::from_bits_truncate(0b10011).contains(BNO055Interrupt::ACC_BSX_DRDY) + ); + assert!(BNO055Interrupt::from_bits_truncate(0b10011).contains(BNO055Interrupt::GYR_DRDY)); + assert!(BNO055Interrupt::from_bits_truncate(0b10011).contains(BNO055Interrupt::MAG_DRDY)); + } +} diff --git a/src/mag_config.rs b/src/mag_config.rs new file mode 100644 index 0000000..0df9e74 --- /dev/null +++ b/src/mag_config.rs @@ -0,0 +1,148 @@ +//! BNO055 Magnetometer Configuration + +use num_derive::FromPrimitive; +use num_traits::FromPrimitive; + +#[derive(Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum Error { + InvalidMagDataRate, + InvalidMagOperationMode, + InvalidMagPowerMode, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, FromPrimitive)] +#[repr(u8)] +pub enum MagDataRate { + Hz2 = 0b000, + Hz6 = 0b001, + Hz8 = 0b010, + Hz10 = 0b011, + Hz15 = 0b100, + Hz20 = 0b101, + Hz25 = 0b110, + Hz30 = 0b111, +} + +#[cfg(feature = "defmt")] +impl defmt::Format for MagDataRate { + fn format(&self, f: defmt::Formatter) { + match self { + MagDataRate::Hz2 => defmt::write!(f, "2 Hz"), + MagDataRate::Hz6 => defmt::write!(f, "6 Hz"), + MagDataRate::Hz8 => defmt::write!(f, "8 Hz"), + MagDataRate::Hz10 => defmt::write!(f, "10 Hz"), + MagDataRate::Hz15 => defmt::write!(f, "15 Hz"), + MagDataRate::Hz20 => defmt::write!(f, "20 Hz"), + MagDataRate::Hz25 => defmt::write!(f, "25 Hz"), + MagDataRate::Hz30 => defmt::write!(f, "30 Hz"), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, FromPrimitive)] +#[repr(u8)] +pub enum MagOperationMode { + LowPower = 0b00, + Regular = 0b01, + EnhancedRegular = 0b10, + HighAccuracy = 0b11, +} + +#[cfg(feature = "defmt")] +impl defmt::Format for MagOperationMode { + fn format(&self, f: defmt::Formatter) { + match self { + MagOperationMode::LowPower => defmt::write!(f, "Low Power"), + MagOperationMode::Regular => defmt::write!(f, "Regular"), + MagOperationMode::EnhancedRegular => defmt::write!(f, "Enhanced Regular"), + MagOperationMode::HighAccuracy => defmt::write!(f, "High Accuracy"), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, FromPrimitive)] +#[repr(u8)] +pub enum MagPowerMode { + Normal = 0b00, + Sleep = 0b01, + Suspend = 0b10, + ForceMode = 0b11, +} + +#[cfg(feature = "defmt")] +impl defmt::Format for MagPowerMode { + fn format(&self, f: defmt::Formatter) { + match self { + MagPowerMode::Normal => defmt::write!(f, "Normal"), + MagPowerMode::Sleep => defmt::write!(f, "Sleep"), + MagPowerMode::Suspend => defmt::write!(f, "Suspend"), + MagPowerMode::ForceMode => defmt::write!(f, "Force Mode"), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub struct MagConfig { + data_rate: MagDataRate, + op_mode: MagOperationMode, + power_mode: MagPowerMode, +} + +impl Default for MagConfig { + fn default() -> Self { + Self { + data_rate: MagDataRate::Hz20, + op_mode: MagOperationMode::Regular, + power_mode: MagPowerMode::Normal, + } + } +} + +impl MagConfig { + pub fn new() -> Self { + Self::default() + } + + pub fn set_data_rate(&mut self, rate: MagDataRate) { + self.data_rate = rate; + } + + pub fn data_rate(&self) -> MagDataRate { + self.data_rate + } + + pub fn set_op_mode(&mut self, mode: MagOperationMode) { + self.op_mode = mode; + } + + pub fn op_mode(&self) -> MagOperationMode { + self.op_mode + } + + pub fn set_power_mode(&mut self, mode: MagPowerMode) { + self.power_mode = mode; + } + + pub fn power_mode(&self) -> MagPowerMode { + self.power_mode + } + + pub fn to_bits(&self) -> u8 { + ((self.power_mode as u8) << 5) | ((self.op_mode as u8) << 3) | (self.data_rate as u8) + } + + pub fn from_bits(bits: u8) -> Result { + let data_rate = MagDataRate::from_u8(bits & 0b111).ok_or(Error::InvalidMagDataRate)?; + let op_mode = + MagOperationMode::from_u8((bits >> 3) & 0b11).ok_or(Error::InvalidMagOperationMode)?; + let power_mode = + MagPowerMode::from_u8((bits >> 5) & 0b11).ok_or(Error::InvalidMagPowerMode)?; + Ok(Self { + data_rate, + op_mode, + power_mode, + }) + } +} diff --git a/src/regs.rs b/src/regs.rs index 108abd9..6914472 100644 --- a/src/regs.rs +++ b/src/regs.rs @@ -84,13 +84,11 @@ pub(crate) const BNO055_OPR_MODE: u8 = 0x3D; pub(crate) const BNO055_PWR_MODE: u8 = 0x3E; pub(crate) const BNO055_SYS_TRIGGER: u8 = 0x3F; -pub(crate) const BNO055_SYS_TRIGGER_RST_SYS_BIT: u8 = 0x20; // Reset command -pub(crate) const BNO055_SYS_TRIGGER_SELF_TEST_BIT: u8 = 0b000_0001; // Self-test command pub(crate) const BNO055_TEMP_SOURCE: u8 = 0x40; pub(crate) const BNO055_AXIS_MAP_CONFIG: u8 = 0x41; pub(crate) const BNO055_AXIS_MAP_SIGN: u8 = 0x42; -/// Calibration data +// Calibration data pub(crate) const BNO055_ACC_OFFSET_X_LSB: u8 = 0x55; pub(crate) const BNO055_ACC_OFFSET_X_MSB: u8 = 0x56; @@ -118,5 +116,28 @@ pub(crate) const BNO055_ACC_RADIUS_MSB: u8 = 0x68; pub(crate) const BNO055_MAG_RADIUS_LSB: u8 = 0x69; pub(crate) const BNO055_MAG_RADIUS_MSB: u8 = 0x6A; +// Interrupts + +pub(crate) const BNO055_INT_MSK: u8 = 0x0F; +pub(crate) const BNO055_INT_EN: u8 = 0x10; +pub(crate) const BNO055_ACC_AM_THRES: u8 = 0x11; +pub(crate) const BNO055_ACC_INT_SETTING: u8 = 0x12; +pub(crate) const BNO055_ACC_HG_DURATION: u8 = 0x13; +pub(crate) const BNO055_ACC_HG_THRES: u8 = 0x14; +pub(crate) const BNO055_ACC_NM_THRES: u8 = 0x15; +pub(crate) const BNO055_ACC_NM_SET: u8 = 0x16; +pub(crate) const BNO055_GYR_INT_SETTING: u8 = 0x17; +pub(crate) const BNO055_GYR_HR_X_SET: u8 = 0x18; +pub(crate) const BNO055_GYR_DUR_X: u8 = 0x19; +pub(crate) const BNO055_GYR_HR_Y_SET: u8 = 0x1A; +pub(crate) const BNO055_GYR_DUR_Y: u8 = 0x1B; +pub(crate) const BNO055_GYR_HR_Z_SET: u8 = 0x1C; +pub(crate) const BNO055_GYR_DUR_Z: u8 = 0x1D; +pub(crate) const BNO055_GYR_AM_THRES: u8 = 0x1E; +pub(crate) const BNO055_GYR_AM_SET: u8 = 0x1F; + /// Sensor config pub(crate) const BNO055_ACC_CONFIG: u8 = 0x08; +pub(crate) const BNO055_MAG_CONFIG: u8 = 0x09; +pub(crate) const BNO055_GYR_CONFIG_0: u8 = 0x0A; +pub(crate) const BNO055_GYR_CONFIG_1: u8 = 0x0B;