Skip to content

Commit

Permalink
Replace Config with explicit register types.
Browse files Browse the repository at this point in the history
  • Loading branch information
@reitermarkus
reitermarkus committed Aug 10, 2022
1 parent fc4df21 commit fc24b44
Show file tree
Hide file tree
Showing 8 changed files with 617 additions and 328 deletions.
97 changes: 28 additions & 69 deletions src/accel_mode_and_odr.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,8 @@ use embedded_hal::blocking::delay::DelayUs;

use crate::{
interface::{ReadData, WriteData},
AccelMode, AccelOutputDataRate, AccelScale, BitFlags as BF, Error, Lsm303agr, Register,
register_address::{CtrlReg1A, CtrlReg4A},
AccelMode, AccelOutputDataRate, AccelScale, Error, Lsm303agr,
};

impl<DI, CommE, PinE, MODE> Lsm303agr<DI, MODE>
Expand All @@ -23,40 +24,17 @@ where
) -> Result<(), Error<CommE, PinE>> {
let old_mode = self.get_accel_mode();

let (mask, lp_only, lp_compat) = match odr {
AccelOutputDataRate::Hz1 => (1 << 4, false, true),
AccelOutputDataRate::Hz10 => (2 << 4, false, true),
AccelOutputDataRate::Hz25 => (3 << 4, false, true),
AccelOutputDataRate::Hz50 => (4 << 4, false, true),
AccelOutputDataRate::Hz100 => (5 << 4, false, true),
AccelOutputDataRate::Hz200 => (6 << 4, false, true),
AccelOutputDataRate::Hz400 => (7 << 4, false, true),
AccelOutputDataRate::Khz1_620LowPower => (8 << 4, true, true),
AccelOutputDataRate::Khz1_344 => (9 << 4, false, false),
AccelOutputDataRate::Khz5_376LowPower => (9 << 4, true, true),
};
let lp_enabled = self.ctrl_reg1_a.is_high(BF::LP_EN);
let hr_enabled = self.ctrl_reg4_a.is_high(BF::HR);
let mut should_lp_be_enabled = lp_enabled;
if lp_enabled {
if !lp_compat {
// HR could not have been enabled.
should_lp_be_enabled = false;
}
} else {
// Currently normal or HR mode
if lp_only {
if hr_enabled {
self.disable_hr()?;
}
// power mode is (now) normal
should_lp_be_enabled = true;
let reg1 = self.ctrl_reg1_a.with_odr(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()?;
}
}
let lp_flag = if should_lp_be_enabled { BF::LP_EN } else { 0 };
let reg1 = (self.ctrl_reg1_a.bits & !(BF::LP_EN | (0x7 << 4))) | mask | lp_flag;
self.iface
.write_accel_register(Register::CTRL_REG1_A, reg1)?;

self.iface.write_accel_register(reg1)?;
self.ctrl_reg1_a = reg1.into();
self.accel_odr = Some(odr);

Expand Down Expand Up @@ -96,10 +74,9 @@ where
self.enable_lp()?;
}
AccelMode::PowerDown => {
let reg1 = self.ctrl_reg1_a.bits & !(0xf << 4);
self.iface
.write_accel_register(Register::CTRL_REG1_A, reg1)?;
self.ctrl_reg1_a = reg1.into();
let reg1 = self.ctrl_reg1_a.difference(CtrlReg1A::ODR);
self.iface.write_accel_register(reg1)?;
self.ctrl_reg1_a = reg1;
self.accel_odr = None;
}
}
Expand All @@ -114,9 +91,9 @@ where

/// Get the accelerometer mode
pub fn get_accel_mode(&mut self) -> AccelMode {
let power_down = (self.ctrl_reg1_a.bits >> 4 & 0xf) == 0;
let lp_enabled = self.ctrl_reg1_a.is_high(BF::LP_EN);
let hr_enabled = self.ctrl_reg4_a.is_high(BF::HR);
let power_down = self.ctrl_reg1_a.intersection(CtrlReg1A::ODR).is_empty();
let lp_enabled = self.ctrl_reg1_a.contains(CtrlReg1A::LPEN);
let hr_enabled = self.ctrl_reg4_a.contains(CtrlReg4A::HR);

if power_down {
AccelMode::PowerDown
Expand All @@ -135,59 +112,41 @@ where
/// `AccelScale::G2` for example can return values between -2g and +2g
/// where g is the gravity of the earth (~9.82 m/s²).
pub fn set_accel_scale(&mut self, scale: AccelScale) -> Result<(), Error<CommE, PinE>> {
let fs = match scale {
AccelScale::G2 => 0b00,
AccelScale::G4 => 0b01,
AccelScale::G8 => 0b10,
AccelScale::G16 => 0b11,
};
let reg4 = self.ctrl_reg4_a.bits & !(0b11 << 4) | (fs << 4);
self.iface
.write_accel_register(Register::CTRL_REG4_A, reg4)?;
let reg4 = self.ctrl_reg4_a.with_scale(scale);
self.iface.write_accel_register(reg4)?;
self.ctrl_reg4_a = reg4.into();
Ok(())
}

/// Get accelerometer scaling factor
pub fn get_accel_scale(&self) -> AccelScale {
let fs = (self.ctrl_reg4_a.bits & (0b11 << 4)) >> 4;
match fs {
0b00 => AccelScale::G2,
0b01 => AccelScale::G4,
0b10 => AccelScale::G8,
0b11 => AccelScale::G16,
_ => unreachable!("bit shift above means we cannot be here"),
}
self.ctrl_reg4_a.scale()
}

fn enable_hr(&mut self) -> Result<(), Error<CommE, PinE>> {
let reg4 = self.ctrl_reg4_a.with_high(BF::HR);
self.iface
.write_accel_register(Register::CTRL_REG4_A, reg4.bits)?;
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.with_low(BF::HR);
self.iface
.write_accel_register(Register::CTRL_REG4_A, reg4.bits)?;
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.with_high(BF::LP_EN);
self.iface
.write_accel_register(Register::CTRL_REG1_A, reg1.bits)?;
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.with_low(BF::LP_EN);
self.iface
.write_accel_register(Register::CTRL_REG1_A, reg1.bits)?;
let reg1 = self.ctrl_reg1_a.difference(CtrlReg1A::LPEN);
self.iface.write_accel_register(reg1)?;
self.ctrl_reg1_a = reg1;
Ok(())
}
Expand Down
81 changes: 42 additions & 39 deletions src/device_impl.rs
View file
Original file line number Diff line number Diff line change
@@ -1,20 +1,25 @@
use crate::{
interface::{I2cInterface, ReadData, SpiInterface, WriteData},
mode, Acceleration, AccelerometerId, BitFlags as BF, Config, Error, Lsm303agr, MagnetometerId,
PhantomData, Register, Status, Temperature, TemperatureStatus,
mode,
register_address::{
CfgRegAM, CfgRegBM, CfgRegCM, CtrlReg1A, CtrlReg4A, StatusRegA, StatusRegAuxA, StatusRegM,
TempCfgRegA, WhoAmIA, WhoAmIM,
},
Acceleration, AccelerometerId, Error, Lsm303agr, MagnetometerId, PhantomData, Status,
Temperature, TemperatureStatus,
};

impl<I2C> Lsm303agr<I2cInterface<I2C>, mode::MagOneShot> {
/// Create new instance of the LSM303AGR device communicating through I2C.
pub fn new_with_i2c(i2c: I2C) -> Self {
Lsm303agr {
iface: I2cInterface { i2c },
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 },
ctrl_reg1_a: CtrlReg1A::default(),
ctrl_reg4_a: CtrlReg4A::default(),
cfg_reg_a_m: CfgRegAM::default(),
cfg_reg_b_m: CfgRegBM::default(),
cfg_reg_c_m: CfgRegCM::default(),
temp_cfg_reg_a: TempCfgRegA::default(),
accel_odr: None,
_mag_mode: PhantomData,
}
Expand All @@ -37,12 +42,12 @@ impl<SPI, CSXL, CSMAG> Lsm303agr<SpiInterface<SPI, CSXL, CSMAG>, mode::MagOneSho
cs_xl: chip_select_accel,
cs_mag: chip_select_mag,
},
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 },
ctrl_reg1_a: CtrlReg1A::default(),
ctrl_reg4_a: CtrlReg4A::default(),
cfg_reg_a_m: CfgRegAM::default(),
cfg_reg_b_m: CfgRegBM::default(),
cfg_reg_c_m: CfgRegCM::default(),
temp_cfg_reg_a: TempCfgRegA::default(),
accel_odr: None,
_mag_mode: PhantomData,
}
Expand All @@ -68,9 +73,8 @@ where

/// Enable block data update for accelerometer.
fn acc_enable_bdu(&mut self) -> Result<(), Error<CommE, PinE>> {
let reg4 = self.ctrl_reg4_a.with_high(BF::ACCEL_BDU);
self.iface
.write_accel_register(Register::CTRL_REG4_A, reg4.bits)?;
let reg4 = self.ctrl_reg4_a | CtrlReg4A::BDU;
self.iface.write_accel_register(reg4)?;
self.ctrl_reg4_a = reg4;

Ok(())
Expand All @@ -81,9 +85,8 @@ where
fn acc_enable_temp(&mut self) -> Result<(), Error<CommE, PinE>> {
self.acc_enable_bdu()?;

let temp_cfg_reg = self.temp_cfg_reg_a.with_high(BF::TEMP_EN);
self.iface
.write_accel_register(Register::TEMP_CFG_REG_A, temp_cfg_reg.bits)?;
let temp_cfg_reg = self.temp_cfg_reg_a | TempCfgRegA::TEMP_EN;
self.iface.write_accel_register(temp_cfg_reg)?;
self.temp_cfg_reg_a = temp_cfg_reg;

Ok(())
Expand All @@ -92,9 +95,17 @@ where
/// Enable block data update for magnetometer.
#[inline]
fn mag_enable_bdu(&mut self) -> Result<(), Error<CommE, PinE>> {
let regc = self.cfg_reg_c_m.with_high(BF::MAG_BDU);
self.iface
.write_mag_register(Register::CFG_REG_C_M, regc.bits)?;
let regc = self.cfg_reg_c_m | CfgRegCM::BDU;
self.iface.write_mag_register(regc)?;
self.cfg_reg_c_m = regc;

Ok(())
}

/// Configure the DRDY pin as a digital output.
pub fn mag_enable_int(&mut self) -> Result<(), Error<CommE, PinE>> {
let regc = self.cfg_reg_c_m | CfgRegCM::INT_MAG;
self.iface.write_mag_register(regc)?;
self.cfg_reg_c_m = regc;

Ok(())
Expand All @@ -103,15 +114,13 @@ where
/// Accelerometer status
pub fn accel_status(&mut self) -> Result<Status, Error<CommE, PinE>> {
self.iface
.read_accel_register(Register::STATUS_REG_A)
.read_accel_register::<StatusRegA>()
.map(Status::new)
}

/// Get measured acceleration.
pub fn acceleration(&mut self) -> Result<Acceleration, Error<CommE, PinE>> {
let (x, y, z) = self
.iface
.read_accel_3_double_registers(Register::OUT_X_L_A)?;
let (x, y, z) = self.iface.read_accel_3_double_registers::<Acceleration>()?;

Ok(Acceleration {
x,
Expand All @@ -125,35 +134,29 @@ where
/// Magnetometer status
pub fn mag_status(&mut self) -> Result<Status, Error<CommE, PinE>> {
self.iface
.read_mag_register(Register::STATUS_REG_M)
.read_mag_register::<StatusRegM>()
.map(Status::new)
}

/// Get the accelerometer device ID.
pub fn accelerometer_id(&mut self) -> Result<AccelerometerId, Error<CommE, PinE>> {
let id = self.iface.read_accel_register(Register::WHO_AM_I_A)?;
Ok(AccelerometerId { raw: id })
self.iface.read_accel_register::<WhoAmIA>()
}

/// Get the magnetometer device ID.
pub fn magnetometer_id(&mut self) -> Result<MagnetometerId, Error<CommE, PinE>> {
let id = self.iface.read_mag_register(Register::WHO_AM_I_M)?;
Ok(MagnetometerId { raw: id })
self.iface.read_mag_register::<WhoAmIM>()
}

/// Get measured temperature.
pub fn temperature(&mut self) -> Result<Temperature, Error<CommE, PinE>> {
let data = self
.iface
.read_accel_double_register(Register::OUT_TEMP_L_A)?;

Ok(Temperature { raw: data })
self.iface.read_accel_double_register::<Temperature>()
}

/// Temperature sensor status
pub fn temperature_status(&mut self) -> Result<TemperatureStatus, Error<CommE, PinE>> {
self.iface
.read_accel_register(Register::STATUS_REG_AUX_A)
.map(TemperatureStatus::new)
.read_accel_register::<StatusRegAuxA>()
.map(|s| TemperatureStatus::new(s.bits()))
}
}

0 comments on commit fc24b44

Please sign in to comment.