From 6c12f1d17b26330ac797eafe694a5daf142997c9 Mon Sep 17 00:00:00 2001 From: Niklas Kapelle Date: Mon, 9 Feb 2026 21:47:45 +0100 Subject: [PATCH] feature parity with cpp lib --- src/lib.rs | 175 ++++++++++++++++++++++++++++++++++++++--------------- 1 file changed, 127 insertions(+), 48 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 811be87..2814637 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -9,15 +9,8 @@ use embedded_hal_async::i2c::{self}; use bitflags::bitflags; -const REG_XOUT_0: u8 = 0x00; -const REG_XOUT_1: u8 = 0x01; -const REG_YOUT_0: u8 = 0x02; -const REG_YOUT_1: u8 = 0x03; -const REG_ZOUT_0: u8 = 0x04; -const REG_ZOUT_1: u8 = 0x05; -const REG_XOUT_2: u8 = 0x06; -const REG_YOUT_2: u8 = 0x07; -const REG_ZOUT_2: u8 = 0x08; +const REG_OUT_START: u8 = 0x00; + const REG_TOUT: u8 = 0x09; const REG_STATUS1: u8 = 0x18; const REG_ODR: u8 = 0x1A; @@ -129,6 +122,18 @@ bitflags! { } } +impl Control1RegisterFlags { + /// Transform bandwidth to bitflags + fn flag_for_bandwidth(bandwidth: Bandwidth) -> Self { + match bandwidth { + Bandwidth::Bw6_6ms => Self::empty(), + Bandwidth::Bw3_5ms => Self::BANDWIDTH_0, + Bandwidth::Bw2_0ms => Self::BANDWIDTH_1, + Bandwidth::Bw1_2ms => Self::BANDWIDTH_0 | Self::BANDWIDTH_1, + } + } +} + bitflags! { /// Flags for the control 2 register #[derive(Debug, Clone, Copy, PartialEq, Eq)] @@ -150,6 +155,7 @@ bitflags! { } } +/// All 3 Axis from a measurement #[derive(Debug, Clone, Copy)] pub struct MagneticMessurement { /// X-Axis in µT @@ -162,17 +168,50 @@ pub struct MagneticMessurement { pub z: f32, } -/// At what rate +/// At what rate to output data pub enum DataRate { + Unset, Hz(u8), Max1000Hz, } +/// Adjust the length of the decimation filter. They control the duration of each measurement. +/// Note: X/Y/Z channel measurements are taken sequentially. Delay Time among those +/// measurements is 1/3 of the Measurement Time defined as the bandwidth. +#[derive(Debug, Clone, Copy)] +enum Bandwidth { + /// 6.6ms + Bw6_6ms, + + /// 3.5ms + Bw3_5ms, + + /// 2ms + Bw2_0ms, + + /// 1.2ms + Bw1_2ms, +} + +impl Bandwidth { + fn to_flags(self) -> Control1RegisterFlags { + Control1RegisterFlags::flag_for_bandwidth(self) + } +} + #[derive(Debug)] pub enum Error { + /// I2C Error I2c(E), + + /// Waiting for a status flag timed out Timeout, + + /// This function is not available in continuous mode NotAvailableInContinuousMode, + + /// You need to set a data rate before using this function + NoDataRateSet, } impl From for Error { @@ -189,7 +228,10 @@ where { i2c: I, delay: D, - is_continuous_mode: bool, + ctrl0: Control0RegisterFlags, + ctrl1: Control1RegisterFlags, + ctrl2: Control2RegisterFlags, + data_rate: DataRate, } impl MMC56X3 @@ -201,7 +243,10 @@ where Self { i2c, delay, - is_continuous_mode: false, + ctrl0: Control0RegisterFlags::empty(), + ctrl1: Control1RegisterFlags::empty(), + ctrl2: Control2RegisterFlags::empty(), + data_rate: DataRate::Unset, } } @@ -217,6 +262,10 @@ where .await?; self.delay.delay_ms(20).await; // According to the datasheet power on time is 20ms + self.ctrl0 = Control0RegisterFlags::empty(); + self.ctrl1 = Control1RegisterFlags::empty(); + self.ctrl2 = Control2RegisterFlags::empty(); + self.magnet_set_reset().await?; self.set_continuous_mode(false).await?; @@ -225,54 +274,85 @@ where /// Pulse large currents through the sense coils to clear any offset pub async fn magnet_set_reset(&mut self) -> Result<(), Error> { - self.write_reg_controll_0(Control0RegisterFlags::DO_SET) + self.write_reg_controll_0(self.ctrl0 | Control0RegisterFlags::DO_SET) .await?; + self.delay.delay_ns(375).await; // According to the datasheet this is how long it takes. - self.write_reg_controll_0(Control0RegisterFlags::empty()) + + self.write_reg_controll_0(self.ctrl0 | Control0RegisterFlags::DO_RESET) .await?; + + self.delay.delay_ns(375).await; + + // No need to undo sets. Bits are self clearing. + Ok(()) } pub async fn set_continuous_mode(&mut self, enable: bool) -> Result<(), Error> { + if matches!(self.data_rate, DataRate::Unset) { + return Err(Error::NoDataRateSet); + } + if enable { - self.write_reg_controll_0(Control0RegisterFlags::CMM_FRE_EN) + self.write_reg_controll_0(self.ctrl0 | Control0RegisterFlags::CMM_FRE_EN) .await?; - self.write_reg_controll_2(Control2RegisterFlags::CMM_EN) - .await?; - self.is_continuous_mode = true; + + self.ctrl2.insert(Control2RegisterFlags::CMM_EN); + self.write_reg_controll_2(self.ctrl2).await?; } else { - self.write_reg_controll_2(Control2RegisterFlags::empty()) - .await?; - self.is_continuous_mode = false; + self.ctrl2.remove(Control2RegisterFlags::CMM_EN); + self.write_reg_controll_2(self.ctrl2).await?; } Ok(()) } + /// Check if the CMM bit is set + pub fn is_continuous_mode(&self) -> bool { + self.ctrl2.contains(Control2RegisterFlags::CMM_EN) + } + pub async fn set_data_rate(&mut self, rate: DataRate) -> Result<(), Error> { match rate { DataRate::Hz(hz) => { self.write_reg_odr(hz).await?; - self.write_reg_controll_2(Control2RegisterFlags::empty()) - .await?; - Ok(()) + self.ctrl2.remove(Control2RegisterFlags::HPOWER); + self.write_reg_controll_2(self.ctrl2).await?; } DataRate::Max1000Hz => { self.write_reg_odr(255).await?; - self.write_reg_controll_2(Control2RegisterFlags::HPOWER) - .await?; - Ok(()) + self.ctrl2.insert(Control2RegisterFlags::HPOWER); + self.write_reg_controll_2(self.ctrl2).await?; + } + DataRate::Unset => { + self.write_reg_odr(0).await?; + if !matches!(self.data_rate, DataRate::Unset) { + self.ctrl2.remove(Control2RegisterFlags::HPOWER); + self.write_reg_controll_2(self.ctrl2).await?; + } } } + + self.data_rate = rate; + + Ok(()) + } + + /// Set the bandwidth selection bits to adjust the length of the decimation filter. They control the duration + /// of each measurement. + #[inline] + async fn set_bandwidth(&mut self, bandwidth: Bandwidth) -> Result<(), Error> { + self.write_reg_controll_1(bandwidth.to_flags()).await } /// Read temperature in Celcius with steps of 0.8 C pub async fn read_temperature(&mut self) -> Result> { - if self.is_continuous_mode { + if self.is_continuous_mode() { return Err(Error::NotAvailableInContinuousMode); } - self.write_reg_controll_0(Control0RegisterFlags::TAKE_MESSUREMENT_T) + self.write_reg_controll_0(self.ctrl0 | Control0RegisterFlags::TAKE_MESSUREMENT_T) .await?; self.wait_for_status_flag(StatusRegisterFlags::MESSUREMENT_T_DONE) @@ -286,9 +366,10 @@ where Ok(temperature) } + /// Read the last measurement pub async fn read_messurement(&mut self) -> Result> { let mut data = [0u8; 9]; - self.read_registers(REG_XOUT_0, &mut data).await?; + self.read_registers(REG_OUT_START, &mut data).await?; let x = ((data[0] as u32) << 12) | ((data[1] as u32) << 4) | ((data[6] as u32) >> 4); let y = ((data[2] as u32) << 12) | ((data[3] as u32) << 4) | ((data[7] as u32) >> 4); @@ -303,26 +384,24 @@ where // Apply resolution. At 20 Bit mode. const RESOLUTION: f32 = 0.00625; - let fx: f32 = x as f32 * RESOLUTION; - let fy: f32 = y as f32 * RESOLUTION; - let fz: f32 = z as f32 * RESOLUTION; + let x: f32 = x as f32 * RESOLUTION; + let y: f32 = y as f32 * RESOLUTION; + let z: f32 = z as f32 * RESOLUTION; - Ok(MagneticMessurement { - x: fx, - y: fy, - z: fz, - }) + Ok(MagneticMessurement { x, y, z }) } + /// Trigger a new measurement if not in continuous mode. + /// Waits for the measurement to complete. pub async fn trigger_messurement(&mut self) -> Result<(), Error> { - self.write_reg_controll_0(Control0RegisterFlags::TAKE_MESSUREMENT_M) + self.write_reg_controll_0(self.ctrl0 | Control0RegisterFlags::TAKE_MESSUREMENT_M) .await?; self.wait_for_status_flag(StatusRegisterFlags::MESSUREMENT_M_DONE) .await } - #[inline(always)] + #[inline] pub async fn read_product_id(&mut self) -> Result> { self.read_register(REG_PRODUCT_ID).await } @@ -342,24 +421,24 @@ where Err(Error::Timeout) } - #[inline(always)] + #[inline] async fn read_reg_temperature(&mut self) -> Result> { self.read_register(REG_TOUT).await } - #[inline(always)] + #[inline] async fn read_reg_status(&mut self) -> Result> { Ok(StatusRegisterFlags::from_bits_truncate( self.read_register(REG_STATUS1).await?, )) } - #[inline(always)] + #[inline] async fn write_reg_odr(&mut self, data: u8) -> Result<(), Error> { self.write_register(REG_ODR, data).await } - #[inline(always)] + #[inline] async fn write_reg_controll_0( &mut self, value: Control0RegisterFlags, @@ -367,7 +446,7 @@ where self.write_register(REG_CONTROL0, value.bits()).await } - #[inline(always)] + #[inline] async fn write_reg_controll_1( &mut self, value: Control1RegisterFlags, @@ -375,7 +454,7 @@ where self.write_register(REG_CONTROL1, value.bits()).await } - #[inline(always)] + #[inline] async fn write_reg_controll_2( &mut self, value: Control2RegisterFlags, @@ -383,7 +462,7 @@ where self.write_register(REG_CONTROL2, value.bits()).await } - #[inline(always)] + #[inline] async fn write_register(&mut self, reg: u8, value: u8) -> Result<(), Error> { self.i2c.write(DEFAULT_ADDRESS, &[reg, value]).await?; Ok(()) @@ -397,7 +476,7 @@ where Ok(data[0]) } - #[inline(always)] + #[inline] async fn read_registers(&mut self, reg: u8, buffer: &mut [u8]) -> Result<(), Error> { self.i2c.write_read(DEFAULT_ADDRESS, &[reg], buffer).await?; Ok(())