Feat/acc config (#24)

* Add methods to get/set accelerometer config

* Fix ACC_Config address

* Fix register name

Cargo.toml

@@ -22,6 +22,8 @@ byteorder = { version = "1", default-features = false }
 serde = { version = "1.0", default-features = false, features = ["derive"], optional = true }
 mint = "^0.5.4"
 bitflags = "1"
+num-traits = "0.2.15"
+num-derive = "0.3.3"
 
 [dependencies.embedded-hal]
 version = "0.2.2"

src/acc_config.rs

@@ -0,0 +1,97 @@
+use num_derive::FromPrimitive;
+use num_traits::FromPrimitive;
+
+const ACC_G_RANGE_MASK: u8 = 0b000_000_11;
+const ACC_BANDWIDTH_MASK: u8 = 0b000_111_00;
+const ACC_OPERATION_MODE_MASK: u8 = 0b111_000_00;
+
+#[derive(Debug)]
+pub enum Error {
+    BadAccGRange,
+    BadAccBandwidth,
+    BadAccOperationMode,
+}
+
+#[derive(Debug, Clone, Copy, FromPrimitive)]
+#[repr(u8)]
+pub enum AccGRange {
+    G2 = 0b000_000_00,
+    G4 = 0b000_000_01,
+    G8 = 0b000_000_10,
+    G16 = 0b000_000_11,
+}
+
+#[derive(Debug, Clone, Copy, FromPrimitive)]
+#[repr(u8)]
+pub enum AccBandwidth {
+    Hz7_81 = 0b000_000_00,
+    Hz15_63 = 0b000_001_00,
+    Hz31_25 = 0b000_010_00,
+    Hz62_5 = 0b000_011_00,
+    Hz125 = 0b000_100_00,
+    Hz250 = 0b000_101_00,
+    Hz500 = 0b000_110_00,
+    Hz1000 = 0b000_111_00,
+}
+
+#[derive(Debug, Clone, Copy, FromPrimitive)]
+#[repr(u8)]
+pub enum AccOperationMode {
+    Normal = 0b000_000_00,
+    Suspend = 0b001_000_00,
+    LowPower1 = 0b010_000_00,
+    Standby = 0b011_000_00,
+    LowPower2 = 0b100_000_00,
+    DeepSuspend = 0b101_000_00,
+}
+
+#[derive(Debug, Clone)]
+pub struct AccConfig {
+    g_range: AccGRange,
+    bandwidth: AccBandwidth,
+    operation_mode: AccOperationMode,
+}
+
+impl AccConfig {
+    pub fn try_from_bits(bits: u8) -> Result<Self, Error> {
+        let g_range = AccGRange::from_u8(bits & ACC_G_RANGE_MASK).ok_or(Error::BadAccGRange)?;
+        let bandwidth =
+            AccBandwidth::from_u8(bits & ACC_BANDWIDTH_MASK).ok_or(Error::BadAccBandwidth)?;
+        let operation_mode = AccOperationMode::from_u8(bits & ACC_OPERATION_MODE_MASK)
+            .ok_or(Error::BadAccOperationMode)?;
+
+        Ok(Self {
+            g_range,
+            bandwidth,
+            operation_mode,
+        })
+    }
+
+    pub fn bits(&self) -> u8 {
+        self.operation_mode as u8 | self.bandwidth as u8 | self.g_range as u8
+    }
+
+    pub fn g_range(&self) -> AccGRange {
+        self.g_range
+    }
+
+    pub fn bandwidth(&self) -> AccBandwidth {
+        self.bandwidth
+    }
+
+    pub fn operation_mode(&self) -> AccOperationMode {
+        self.operation_mode
+    }
+
+    pub fn set_g_range(&mut self, g_range: AccGRange) {
+        self.g_range = g_range;
+    }
+
+    pub fn set_bandwidth(&mut self, bandwidth: AccBandwidth) {
+        self.bandwidth = bandwidth;
+    }
+
+    pub fn set_operation_mode(&mut self, operation_mode: AccOperationMode) {
+        self.operation_mode = operation_mode;
+    }
+}

src/lib.rs

@@ -14,10 +14,12 @@ pub use mint;
 #[cfg(feature = "serde")]
 use serde::{Deserialize, Serialize};
 
+mod acc_config;
 mod regs;
 #[cfg(feature = "std")]
 mod std;
 
+pub use acc_config::{AccBandwidth, AccConfig, AccGRange, AccOperationMode};
 pub use regs::BNO055_ID;
 
 /// All possible errors in this crate
@@ -31,6 +33,9 @@ pub enum Error<E> {
 
     /// Invalid (not applicable) device mode.
     InvalidMode,
+
+    /// Accelerometer configuration error
+    AccConfig(acc_config::Error),
 }
 
 pub struct Bno055<I> {
@@ -462,6 +467,25 @@ where
         Ok(self.mode.is_fusion_enabled())
     }
 
+    pub fn get_acc_config(&mut self) -> Result<AccConfig, Error<E>> {
+        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)
+    }
+
+    pub fn set_acc_config(&mut self, acc_config: &AccConfig) -> Result<(), Error<E>> {
+        self.set_page(BNO055RegisterPage::PAGE_1)?;
+
+        self.write_u8(regs::BNO055_ACC_CONFIG, acc_config.bits())
+            .map_err(Error::I2c)?;
+
+        Ok(())
+    }
+
     /// Sets current register map page.
     fn set_page(&mut self, page: BNO055RegisterPage) -> Result<(), Error<E>> {
         self.write_u8(regs::BNO055_PAGE_ID, page.bits())

src/regs.rs

@@ -117,3 +117,6 @@ pub(crate) const BNO055_ACC_RADIUS_LSB: u8 = 0x67;
 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;
+
+/// Sensor config
+pub(crate) const BNO055_ACC_CONFIG: u8 = 0x08;

src/std.rs

@@ -1,4 +1,4 @@
-use crate::Error;
+use crate::{acc_config, Error};
 use std::{error, fmt};
 
 impl<E: core::fmt::Debug> error::Error for Error<E> {}
@@ -8,3 +8,11 @@ impl<E: core::fmt::Debug> fmt::Display for Error<E> {
         write!(f, "{:?}", self)
     }
 }
+
+impl error::Error for acc_config::Error {}
+
+impl fmt::Display for acc_config::Error {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        write!(f, "{:?}", self)
+    }
+}