Merge 0491b1d into dbb4e32

eldruin · Sep 11, 2022 · 49814e0 · 49814e0
2 parents dbb4e32 + 0491b1d
commit 49814e0
Show file tree

Hide file tree

Showing 9 changed files with 179 additions and 267 deletions.
diff --git a/README.md b/README.md
@@ -15,8 +15,7 @@ This driver allows you to:
 - Accelerometer:
     - Read measured acceleration. See: `acceleration()`.
     - Get accelerometer status. See: `accel_status()`.
-    - Set accelerometer output data rate. See: `set_accel_odr()`.
-    - Set accelerometer mode. See: `set_accel_mode()`.
+    - Set accelerometer mode and output data rate. See: `set_accel_mode_and_odr()`.
     - Set accelerometer scale. See: `set_accel_scale()`.
     - Get accelerometer ID. See: `accelerometer_id()`.
     - Get temperature sensor status. See: `temperature_status()`.
@@ -27,11 +26,10 @@ This driver allows you to:
     - Get the magnetometer status. See: `mag_status()`.
     - Change into continuous/one-shot mode. See: `into_mag_continuous()`.
     - Read measured magnetic field. See: `magnetic_field()`.
-    - Set magnetometer output data rate. See: `set_mag_odr()`.
+    - Set magnetometer mode and output data rate. See: `set_mag_mode_and_odr()`.
     - Get magnetometer ID. See: `magnetometer_id()`.
     - Enable/disable magnetometer built in offset cancellation. See: `enable_mag_offset_cancellation()`.
     - Enable/disable magnetometer low-pass filter. See: `mag_enable_low_pass_filter()`.
-    - Set magnetometer mode. See: `set_mag_mode()`.
 
 <!-- TODO
 [Introductory blog post]()
@@ -64,13 +62,13 @@ Please find additional examples using hardware in this repository: [driver-examp
 
 ```rust
 use linux_embedded_hal::{Delay, I2cdev};
-use lsm303agr::{AccelOutputDataRate, Lsm303agr};
+use lsm303agr::{AccelMode, AccelOutputDataRate, Lsm303agr};
 
 fn main() {
     let dev = I2cdev::new("/dev/i2c-1").unwrap();
     let mut sensor = Lsm303agr::new_with_i2c(dev);
     sensor.init().unwrap();
-    sensor.set_accel_odr(&mut Delay, AccelOutputDataRate::Hz50).unwrap();
+    sensor.set_accel_mode_and_odr(&mut Delay, AccelMode::Normal, AccelOutputDataRate::Hz50).unwrap();
     loop {
         if sensor.accel_status().unwrap().xyz_new_data() {
             let data = sensor.acceleration().unwrap();

diff --git a/examples/linux.rs b/examples/linux.rs
@@ -1,13 +1,13 @@
 #[cfg(target_os = "linux")]
 fn main() {
     use linux_embedded_hal::{Delay, I2cdev};
-    use lsm303agr::{AccelOutputDataRate, Lsm303agr};
+    use lsm303agr::{AccelMode, AccelOutputDataRate, Lsm303agr};
 
     let dev = I2cdev::new("/dev/i2c-1").unwrap();
     let mut sensor = Lsm303agr::new_with_i2c(dev);
     sensor.init().unwrap();
     sensor
-        .set_accel_odr(&mut Delay, AccelOutputDataRate::Hz50)
+        .set_accel_mode_and_odr(&mut Delay, AccelMode::Normal, AccelOutputDataRate::Hz50)
         .unwrap();
     loop {
         if sensor.accel_status().unwrap().xyz_new_data() {

diff --git a/src/accel_mode_and_odr.rs b/src/accel_mode_and_odr.rs
@@ -10,75 +10,51 @@ impl<DI, CommE, PinE, MODE> Lsm303agr<DI, MODE>
 where
     DI: ReadData<Error = Error<CommE, PinE>> + WriteData<Error = Error<CommE, PinE>>,
 {
-    /// Set accelerometer output data rate.
+    /// Set accelerometer power/resolution mode and output data rate.
     ///
-    /// This changes the power mode if the current one is not appropriate.
-    /// When changing from a low-power-only output data rate setting into
-    /// a high-resolution or normal power mode, it changes into normal mode.
+    /// Returns `Error::InvalidInputData` if the mode is incompatible with
+    /// the given output data rate.
     ///
     #[doc = include_str!("delay.md")]
-    pub fn set_accel_odr<D: DelayUs<u32>>(
+    pub fn set_accel_mode_and_odr<D: DelayUs<u32>>(
         &mut self,
         delay: &mut D,
-        odr: AccelOutputDataRate,
+        mode: AccelMode,
+        odr: impl Into<Option<AccelOutputDataRate>>,
     ) -> Result<(), Error<CommE, PinE>> {
+        let odr = odr.into();
+
+        check_accel_odr_is_compatible_with_mode(odr, mode)?;
+
         let old_mode = self.get_accel_mode();
 
-        let reg1 = self.ctrl_reg1_a.with_odr(odr);
+        let mut reg1 = self.ctrl_reg1_a.difference(CtrlReg1A::ODR);
 
-        // Check if low-power mode should be enabled.
-        if reg1.contains(CtrlReg1A::LPEN) {
-            // Disable high-resolution mode if it is enabled.
-            if self.ctrl_reg4_a.contains(CtrlReg4A::HR) {
-                self.disable_hr()?;
-            }
+        if let Some(odr) = odr {
+            reg1 = reg1.with_odr(odr);
         }
 
-        self.iface.write_accel_register(reg1)?;
-        self.ctrl_reg1_a = reg1;
-        self.accel_odr = Some(odr);
-
-        let mode = self.get_accel_mode();
-        let change_time = old_mode.change_time_us(mode, odr);
-        delay.delay_us(change_time);
+        let reg1 = if mode == AccelMode::LowPower {
+            reg1.union(CtrlReg1A::LPEN)
+        } else {
+            reg1.difference(CtrlReg1A::LPEN)
+        };
 
-        Ok(())
-    }
+        let reg4 = self.ctrl_reg4_a.difference(CtrlReg4A::HR);
 
-    /// Set accelerometer power/resolution mode
-    ///
-    /// Returns `Error::InvalidInputData` if the mode is incompatible with the current
-    /// accelerometer output data rate.
-    ///
-    #[doc = include_str!("delay.md")]
-    pub fn set_accel_mode<D: DelayUs<u32>>(
-        &mut self,
-        delay: &mut D,
-        mode: AccelMode,
-    ) -> Result<(), Error<CommE, PinE>> {
-        check_accel_odr_is_compatible_with_mode(self.accel_odr, mode)?;
+        if mode != AccelMode::HighResolution {
+            self.iface.write_accel_register(reg4)?;
+            self.ctrl_reg4_a = reg4;
+        }
 
-        let old_mode = self.get_accel_mode();
+        self.iface.write_accel_register(reg1)?;
+        self.ctrl_reg1_a = reg1;
+        self.accel_odr = odr;
 
-        match mode {
-            AccelMode::HighResolution => {
-                self.disable_lp()?;
-                self.enable_hr()?;
-            }
-            AccelMode::Normal => {
-                self.disable_lp()?;
-                self.disable_hr()?;
-            }
-            AccelMode::LowPower => {
-                self.disable_hr()?;
-                self.enable_lp()?;
-            }
-            AccelMode::PowerDown => {
-                let reg1 = self.ctrl_reg1_a.difference(CtrlReg1A::ODR);
-                self.iface.write_accel_register(reg1)?;
-                self.ctrl_reg1_a = reg1;
-                self.accel_odr = None;
-            }
+        if mode == AccelMode::HighResolution {
+            let reg4 = reg4.union(CtrlReg4A::HR);
+            self.iface.write_accel_register(reg4)?;
+            self.ctrl_reg4_a = reg4;
         }
 
         if let Some(odr) = self.accel_odr {
@@ -122,48 +98,23 @@ where
     pub fn get_accel_scale(&self) -> AccelScale {
         self.ctrl_reg4_a.scale()
     }
-
-    fn enable_hr(&mut self) -> Result<(), Error<CommE, PinE>> {
-        let reg4 = self.ctrl_reg4_a.union(CtrlReg4A::HR);
-        self.iface.write_accel_register(reg4)?;
-        self.ctrl_reg4_a = reg4;
-        Ok(())
-    }
-
-    fn disable_hr(&mut self) -> Result<(), Error<CommE, PinE>> {
-        let reg4 = self.ctrl_reg4_a.difference(CtrlReg4A::HR);
-        self.iface.write_accel_register(reg4)?;
-        self.ctrl_reg4_a = reg4;
-        Ok(())
-    }
-
-    fn enable_lp(&mut self) -> Result<(), Error<CommE, PinE>> {
-        let reg1 = self.ctrl_reg1_a.union(CtrlReg1A::LPEN);
-        self.iface.write_accel_register(reg1)?;
-        self.ctrl_reg1_a = reg1;
-        Ok(())
-    }
-
-    fn disable_lp(&mut self) -> Result<(), Error<CommE, PinE>> {
-        let reg1 = self.ctrl_reg1_a.difference(CtrlReg1A::LPEN);
-        self.iface.write_accel_register(reg1)?;
-        self.ctrl_reg1_a = reg1;
-        Ok(())
-    }
 }
 
 fn check_accel_odr_is_compatible_with_mode<CommE, PinE>(
     odr: Option<AccelOutputDataRate>,
     mode: AccelMode,
 ) -> Result<(), Error<CommE, PinE>> {
-    if (odr == Some(AccelOutputDataRate::Khz1_620LowPower)
-        || odr == Some(AccelOutputDataRate::Khz5_376LowPower))
-        && (mode == AccelMode::Normal || mode == AccelMode::HighResolution)
-        || (odr == Some(AccelOutputDataRate::Khz1_344) && mode == AccelMode::LowPower)
-    {
-        Err(Error::InvalidInputData)
-    } else {
-        Ok(())
+    match (odr, mode) {
+        (None, AccelMode::PowerDown) => Ok(()),
+        (None, _) => Err(Error::InvalidInputData),
+        (Some(odr), mode) => match (odr, mode) {
+            (AccelOutputDataRate::Khz1_344, AccelMode::LowPower)
+            | (
+                AccelOutputDataRate::Khz1_620LowPower | AccelOutputDataRate::Khz5_376LowPower,
+                AccelMode::Normal | AccelMode::HighResolution,
+            ) => Err(Error::InvalidInputData),
+            _ => Ok(()),
+        },
     }
 }
 
@@ -193,6 +144,13 @@ mod accel_odr_mode_tests {
         };
     }
 
+    macro_rules! not_none_odr_compatible {
+        ($power:ident) => {
+            check_accel_odr_is_compatible_with_mode::<(), ()>(None, AccelMode::$power)
+                .expect_err("Shout not be compatible");
+        };
+    }
+
     #[test]
     fn all_modes_are_compatible_with_powerdown() {
         compatible!(Hz1, PowerDown);
@@ -251,9 +209,9 @@ mod accel_odr_mode_tests {
 
     #[test]
     fn none_odr_compatibility() {
-        none_odr_compatible!(LowPower);
-        none_odr_compatible!(Normal);
-        none_odr_compatible!(HighResolution);
+        not_none_odr_compatible!(LowPower);
+        not_none_odr_compatible!(Normal);
+        not_none_odr_compatible!(HighResolution);
         none_odr_compatible!(PowerDown);
     }
 }
diff --git a/src/lib.rs b/src/lib.rs
@@ -10,8 +10,7 @@
 //! - Accelerometer:
 //!     - Read measured acceleration. See: [`acceleration()`](Lsm303agr::acceleration).
 //!     - Get accelerometer status. See: [`accel_status()`](Lsm303agr::accel_status).
-//!     - Set accelerometer output data rate. See: [`set_accel_odr()`](Lsm303agr::set_accel_odr).
-//!     - Set accelerometer mode. See: [`set_accel_mode()`](Lsm303agr::set_accel_mode).
+//!     - Set accelerometer mode and output data rate. See: [`set_accel_mode_and_odr()`](Lsm303agr::set_accel_mode_and_odr).
 //!     - Set accelerometer scale. See: [`set_accel_scale()`](Lsm303agr::set_accel_scale).
 //!     - Get accelerometer ID. See: [`accelerometer_id()`](Lsm303agr::accelerometer_id).
 //!     - Get temperature sensor status. See: [`temperature_status()`](Lsm303agr::temperature_status).
@@ -22,11 +21,10 @@
 //!     - Get the magnetometer status. See: [`mag_status()`](Lsm303agr::mag_status).
 //!     - Change into continuous/one-shot mode. See: [`into_mag_continuous()`](Lsm303agr::into_mag_continuous).
 //!     - Read measured magnetic field. See: [`magnetic_field()`](Lsm303agr::magnetic_field).
-//!     - Set magnetometer output data rate. See: [`set_mag_odr()`](Lsm303agr::set_mag_odr).
+//!     - Set magnetometer mode and output data rate. See: [`set_mag_mode_and_odr()`](Lsm303agr::set_mag_mode_and_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).
 //!     - Enable/disable magnetometer low-pass filter. See: [`mag_enable_low_pass_filter()`](Lsm303agr::mag_enable_low_pass_filter).
-//!     - Set magnetometer mode. See: [`set_mag_mode()`](Lsm303agr::set_mag_mode).
 //!
 //! <!-- TODO
 //! [Introductory blog post](TODO)
@@ -74,13 +72,13 @@
 //! ```no_run
 //! # #[cfg(target_os = "linux")] {
 //! use linux_embedded_hal::{Delay, I2cdev};
-//! use lsm303agr::{AccelOutputDataRate, Lsm303agr};
+//! use lsm303agr::{AccelMode, AccelOutputDataRate, Lsm303agr};
 //!
 //! let dev = I2cdev::new("/dev/i2c-1").unwrap();
 //! let mut sensor = Lsm303agr::new_with_i2c(dev);
 //!
 //! sensor.init().unwrap();
-//! sensor.set_accel_odr(&mut Delay, AccelOutputDataRate::Hz10).unwrap();
+//! sensor.set_accel_mode_and_odr(&mut Delay, AccelMode::Normal, AccelOutputDataRate::Hz10).unwrap();
 //!
 //! loop {
 //!     if sensor.accel_status().unwrap().xyz_new_data() {
@@ -96,15 +94,15 @@
 //! ```no_run
 //! # #[cfg(target_os = "linux")] {
 //! use linux_embedded_hal::{Delay, Spidev, Pin};
-//! use lsm303agr::{AccelOutputDataRate, Lsm303agr};
+//! use lsm303agr::{AccelMode, AccelOutputDataRate, Lsm303agr};
 //!
 //! let dev = Spidev::open("/dev/spidev0.0").unwrap();
 //! 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(&mut Delay, AccelOutputDataRate::Hz10).unwrap();
+//! sensor.set_accel_mode_and_odr(&mut Delay, AccelMode::Normal, AccelOutputDataRate::Hz10).unwrap();
 //!
 //! loop {
 //!     if sensor.accel_status().unwrap().xyz_new_data() {

diff --git a/src/magnetometer.rs b/src/magnetometer.rs
@@ -11,53 +11,35 @@ impl<DI, CommE, PinE, MODE> Lsm303agr<DI, MODE>
 where
     DI: ReadData<Error = Error<CommE, PinE>> + WriteData<Error = Error<CommE, PinE>>,
 {
-    /// Set magnetometer output data rate.
+    /// Set magnetometer power/resolution mode and output data rate.
     ///
     #[doc = include_str!("delay.md")]
-    pub fn set_mag_odr<D: DelayUs<u32>>(
+    pub fn set_mag_mode_and_odr<D: DelayUs<u32>>(
         &mut self,
         delay: &mut D,
+        mode: MagMode,
         odr: MagOutputDataRate,
     ) -> Result<(), Error<CommE, PinE>> {
         let rega = self.cfg_reg_a_m;
 
+        let old_mode = rega.mode();
         let old_odr = rega.odr();
 
-        let rega = rega.with_odr(odr);
+        let rega = rega.with_mode(mode).with_odr(odr);
         self.iface.write_mag_register(rega)?;
         self.cfg_reg_a_m = rega;
 
-        if old_odr != odr && self.cfg_reg_b_m.offset_cancellation() {
+        let offset_cancellation = self.cfg_reg_b_m.offset_cancellation();
+        if old_mode != mode {
+            delay.delay_us(rega.turn_on_time_us(offset_cancellation));
+        } else if old_odr != odr && offset_cancellation {
             // Mode did not change, so only wait for 1/ODR ms.
             delay.delay_us(odr.turn_on_time_us_frac_1());
         }
 
         Ok(())
     }
 
-    /// Set magnetometer power mode.
-    ///
-    #[doc = include_str!("delay.md")]
-    pub fn set_mag_mode<D: DelayUs<u32>>(
-        &mut self,
-        delay: &mut D,
-        mode: MagMode,
-    ) -> Result<(), Error<CommE, PinE>> {
-        let rega = self.cfg_reg_a_m;
-
-        let old_mode = rega.mode();
-
-        let rega = rega.with_mode(mode);
-        self.iface.write_mag_register(rega)?;
-        self.cfg_reg_a_m = rega;
-
-        if old_mode != mode {
-            delay.delay_us(rega.turn_on_time_us(self.cfg_reg_b_m.offset_cancellation()));
-        }
-
-        Ok(())
-    }
-
     /// Get magnetometer power/resolution mode.
     pub fn get_mag_mode(&self) -> MagMode {
         self.cfg_reg_a_m.mode()