Magnetometer offset cancellation (#10)

* Magnetometer offset cancellation * Clarify the docs of the offset cancellation * Seperate enable and disable offset cancellation functions * utilize BitFlags and with_{low,high}() * Documentation about the changes * Add tests for magnetometer offset cancellation * Allow tests to run on non-linux platforms - Make linux-specific doc-tests ignored on non-linux platforms - Make linux.rs example compile on non-linux platforms * fix bad formatting * Revert "Allow tests to run on non-linux platforms" This reverts commit 6697faf.
eldruin · Dec 28, 2021 · 746b6ac · 746b6ac
1 parent ba698f7
commit 746b6ac
Show file tree

Hide file tree

Showing 9 changed files with 156 additions and 1 deletion.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -7,6 +7,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/spec/v2.0.0.
 
 ## [Unreleased]
 - Support temperature sensor.
+- Support magnetometer offset cancellation
 
 ...
 ## [0.2.2] - 2021-09-21

diff --git a/README.md b/README.md
@@ -30,6 +30,7 @@ This driver allows you to:
     - Read magnetometer data unscaled. See: `mag_data_unscaled()`.
     - Set magnetometer output data rate. See: `set_mag_odr()`.
     - Get magnetometer ID. See: `magnetometer_id()`.
+    - Enable/disable magnetometer built in offset cancellation. See: `enable_mag_offset_cancellation()`.
 
 <!-- TODO
 [Introductory blog post]()

diff --git a/src/device_impl.rs b/src/device_impl.rs
@@ -14,6 +14,7 @@ impl<I2C> Lsm303agr<I2cInterface<I2C>, mode::MagOneShot> {
             ctrl_reg1_a: Config { bits: 0x7 },
             ctrl_reg4_a: Config { bits: 0 },
             cfg_reg_a_m: Config { bits: 0x3 },
+            cfg_reg_b_m: Config { bits: 0 },
             cfg_reg_c_m: Config { bits: 0 },
             temp_cfg_reg_a: Config { bits: 0 },
             accel_odr: None,
@@ -41,6 +42,7 @@ impl<SPI, CSXL, CSMAG> Lsm303agr<SpiInterface<SPI, CSXL, CSMAG>, mode::MagOneSho
             ctrl_reg1_a: Config { bits: 0x7 },
             ctrl_reg4_a: Config { bits: 0 },
             cfg_reg_a_m: Config { bits: 0x3 },
+            cfg_reg_b_m: Config { bits: 0 },
             cfg_reg_c_m: Config { bits: 0 },
             temp_cfg_reg_a: Config { bits: 0 },
             accel_odr: None,

diff --git a/src/lib.rs b/src/lib.rs
@@ -25,6 +25,7 @@
 //!     - Read magnetometer data unscaled. See: [`mag_data()`](Lsm303agr::mag_data_unscaled).
 //!     - Set magnetometer output data rate. See: [`set_mag_odr()`](Lsm303agr::set_mag_odr).
 //!     - Get magnetometer ID. See: [`magnetometer_id()`](Lsm303agr::magnetometer_id).
+//!     - Enable/disable magnetometer built in offset cancellation. See: [`enable_mag_offset_cancellation()`](Lsm303agr::enable_mag_offset_cancellation).
 //!
 //! <!-- TODO
 //! [Introductory blog post](TODO)
@@ -75,8 +76,10 @@
 //!
 //! let dev = I2cdev::new("/dev/i2c-1").unwrap();
 //! let mut sensor = Lsm303agr::new_with_i2c(dev);
+//!
 //! sensor.init().unwrap();
 //! sensor.set_accel_odr(AccelOutputDataRate::Hz10).unwrap();
+//!
 //! loop {
 //!     if sensor.accel_status().unwrap().xyz_new_data {
 //!         let data = sensor.accel_data().unwrap();
@@ -95,8 +98,10 @@
 //! let accel_cs = Pin::new(17);
 //! let mag_cs = Pin::new(27);
 //! let mut sensor = Lsm303agr::new_with_spi(dev, accel_cs, mag_cs);
+//!
 //! sensor.init().unwrap();
 //! sensor.set_accel_odr(AccelOutputDataRate::Hz10).unwrap();
+//!
 //! loop {
 //!     if sensor.accel_status().unwrap().xyz_new_data {
 //!         let data = sensor.accel_data().unwrap();
@@ -131,6 +136,7 @@ pub struct Lsm303agr<DI, MODE> {
     ctrl_reg1_a: Config,
     ctrl_reg4_a: Config,
     cfg_reg_a_m: Config,
+    cfg_reg_b_m: Config,
     cfg_reg_c_m: Config,
     temp_cfg_reg_a: Config,
     accel_odr: Option<AccelOutputDataRate>,

diff --git a/src/mag_mode_change.rs b/src/mag_mode_change.rs
@@ -19,6 +19,7 @@ where
                 ctrl_reg1_a: self.ctrl_reg1_a,
                 ctrl_reg4_a: self.ctrl_reg4_a,
                 cfg_reg_a_m: cfg.into(),
+                cfg_reg_b_m: self.cfg_reg_b_m,
                 cfg_reg_c_m: self.cfg_reg_c_m,
                 temp_cfg_reg_a: self.temp_cfg_reg_a,
                 accel_odr: None,
@@ -47,6 +48,7 @@ where
                 ctrl_reg1_a: self.ctrl_reg1_a,
                 ctrl_reg4_a: self.ctrl_reg4_a,
                 cfg_reg_a_m: cfg.into(),
+                cfg_reg_b_m: self.cfg_reg_b_m,
                 cfg_reg_c_m: self.cfg_reg_c_m,
                 temp_cfg_reg_a: self.temp_cfg_reg_a,
                 accel_odr: None,

diff --git a/src/magnetometer.rs b/src/magnetometer.rs
@@ -1,6 +1,8 @@
 use crate::{
     interface::{ReadData, WriteData},
-    mode, Error, Lsm303agr, MagOutputDataRate, Measurement, Register, UnscaledMeasurement,
+    mode,
+    register_address::BitFlags,
+    Error, Lsm303agr, MagOutputDataRate, Measurement, Register, UnscaledMeasurement,
 };
 
 impl<DI, CommE, PinE, MODE> Lsm303agr<DI, MODE>
@@ -54,6 +56,32 @@ where
             z: data.2 as i16,
         })
     }
+
+    /// Enable the magnetometer's built in offset cancellation.
+    ///
+    /// Offset cancellation is **automatically** managed by the device in **continuous** mode.
+    ///
+    /// To later disable offset cancellation, use the [`disable_mag_offset_cancellation`](Lsm303agr::disable_mag_offset_cancellation) function
+    pub fn enable_mag_offset_cancellation(&mut self) -> Result<(), Error<CommE, PinE>> {
+        let reg_b = self.cfg_reg_b_m.with_high(BitFlags::MAG_OFF_CANC);
+
+        self.iface
+            .write_mag_register(Register::CFG_REG_B_M, reg_b.bits)?;
+        self.cfg_reg_b_m = reg_b;
+
+        Ok(())
+    }
+
+    /// Disable the magnetometer's built in offset cancellation.
+    pub fn disable_mag_offset_cancellation(&mut self) -> Result<(), Error<CommE, PinE>> {
+        let reg_b = self.cfg_reg_b_m.with_low(BitFlags::MAG_OFF_CANC);
+
+        self.iface
+            .write_mag_register(Register::CFG_REG_B_M, reg_b.bits)?;
+        self.cfg_reg_b_m = reg_b;
+
+        Ok(())
+    }
 }
 
 impl<DI, CommE, PinE> Lsm303agr<DI, mode::MagOneShot>
@@ -92,6 +120,39 @@ where
             Err(nb::Error::WouldBlock)
         }
     }
+
+    /// Enable the magnetometer's built in offset cancellation.
+    ///
+    /// Offset cancellation has to be **managed by the user** in **single measurement** (OneShot) mode averaging
+    /// two consecutive measurements H<sub>n</sub> and H<sub>n-1</sub>.
+    ///
+    /// To later disable offset cancellation, use the [`disable_mag_offset_cancellation`](Lsm303agr::disable_mag_offset_cancellation) function
+    pub fn enable_mag_offset_cancellation(&mut self) -> Result<(), Error<CommE, PinE>> {
+        let reg_b = self
+            .cfg_reg_b_m
+            .with_high(BitFlags::MAG_OFF_CANC)
+            .with_high(BitFlags::MAG_OFF_CANC_ONE_SHOT);
+
+        self.iface
+            .write_mag_register(Register::CFG_REG_B_M, reg_b.bits)?;
+        self.cfg_reg_b_m = reg_b;
+
+        Ok(())
+    }
+
+    /// Disable the magnetometer's built in offset cancellation.
+    pub fn disable_mag_offset_cancellation(&mut self) -> Result<(), Error<CommE, PinE>> {
+        let reg_b = self
+            .cfg_reg_b_m
+            .with_low(BitFlags::MAG_OFF_CANC)
+            .with_low(BitFlags::MAG_OFF_CANC_ONE_SHOT);
+
+        self.iface
+            .write_mag_register(Register::CFG_REG_B_M, reg_b.bits)?;
+        self.cfg_reg_b_m = reg_b;
+
+        Ok(())
+    }
 }
 
 const SCALING_FACTOR: i32 = 150;

diff --git a/src/register_address.rs b/src/register_address.rs
@@ -10,6 +10,7 @@ impl Register {
     pub const OUT_X_L_A: u8 = 0x28;
     pub const WHO_AM_I_M: u8 = 0x4F;
     pub const CFG_REG_A_M: u8 = 0x60;
+    pub const CFG_REG_B_M: u8 = 0x61;
     pub const CFG_REG_C_M: u8 = 0x62;
     pub const STATUS_REG_M: u8 = 0x67;
     pub const OUTX_L_REG_M: u8 = 0x68;
@@ -30,6 +31,9 @@ impl BitFlags {
 
     pub const MAG_BDU: u8 = 1 << 4;
 
+    pub const MAG_OFF_CANC: u8 = 1 << 1;
+    pub const MAG_OFF_CANC_ONE_SHOT: u8 = 1 << 4;
+
     pub const XDR: u8 = 1;
     pub const YDR: u8 = 1 << 1;
     pub const ZDR: u8 = 1 << 2;

diff --git a/tests/common/mod.rs b/tests/common/mod.rs
@@ -29,6 +29,7 @@ impl Register {
     pub const OUT_X_L_A: u8 = 0x28;
     pub const WHO_AM_I_M: u8 = 0x4F;
     pub const CFG_REG_A_M: u8 = 0x60;
+    pub const CFG_REG_B_M: u8 = 0x61;
     pub const CFG_REG_C_M: u8 = 0x62;
     pub const STATUS_REG_M: u8 = 0x67;
     pub const OUTX_L_REG_M: u8 = 0x68;
@@ -50,6 +51,9 @@ impl BitFlags {
 
     pub const MAG_BDU: u8 = 1 << 4;
 
+    pub const MAG_OFF_CANC: u8 = 1 << 1;
+    pub const MAG_OFF_CANC_ONE_SHOT: u8 = 1 << 4;
+
     pub const XDR: u8 = 1;
     pub const YDR: u8 = 1 << 1;
     pub const ZDR: u8 = 1 << 2;

diff --git a/tests/magnetometer.rs b/tests/magnetometer.rs
@@ -165,3 +165,77 @@ fn can_take_continuous_measurement_spi() {
     );
     destroy_spi(sensor);
 }
+
+#[test]
+fn can_enable_mag_offset_cancellation_continuous() {
+    let sensor = new_i2c(&[
+        // Mag continuous mode
+        I2cTrans::write(MAG_ADDR, vec![Register::CFG_REG_A_M, 0]),
+        // Enable offset cancellation
+        I2cTrans::write(MAG_ADDR, vec![Register::CFG_REG_B_M, BF::MAG_OFF_CANC]),
+    ]);
+
+    let mut sensor = sensor
+        .into_mag_continuous()
+        .expect("failed to set magnetometer into continuous mode");
+
+    sensor
+        .enable_mag_offset_cancellation()
+        .expect("failed to enable offset cancellation");
+
+    destroy_i2c(sensor);
+}
+
+#[test]
+fn can_disable_mag_offset_cancellation_continuous() {
+    let sensor = new_i2c(&[
+        // Mag continuous mode
+        I2cTrans::write(MAG_ADDR, vec![Register::CFG_REG_A_M, 0]),
+        // Disable offset cancellation
+        I2cTrans::write(MAG_ADDR, vec![Register::CFG_REG_B_M, 0]),
+    ]);
+
+    let mut sensor = sensor
+        .into_mag_continuous()
+        .expect("failed to set magnetometer into continuous mode");
+
+    sensor
+        .disable_mag_offset_cancellation()
+        .expect("failed to disable offset cancellation");
+
+    destroy_i2c(sensor);
+}
+
+#[test]
+fn can_enable_mag_offset_cancellation_one_shot() {
+    let mut sensor = new_i2c(&[
+        // Enable offset cancellation
+        I2cTrans::write(
+            MAG_ADDR,
+            vec![
+                Register::CFG_REG_B_M,
+                BF::MAG_OFF_CANC | BF::MAG_OFF_CANC_ONE_SHOT,
+            ],
+        ),
+    ]);
+
+    sensor
+        .enable_mag_offset_cancellation()
+        .expect("failed to disable offset cancellation");
+
+    destroy_i2c(sensor);
+}
+
+#[test]
+fn can_disable_mag_offset_cancellation_one_shot() {
+    let mut sensor = new_i2c(&[
+        // Disable offset cancellation
+        I2cTrans::write(MAG_ADDR, vec![Register::CFG_REG_B_M, 0]),
+    ]);
+
+    sensor
+        .disable_mag_offset_cancellation()
+        .expect("failed to disable offset cancellation");
+
+    destroy_i2c(sensor);
+}