kxtj3_1057/
lib.rs

1//! Platform-agnostic  KXTJ3-1057 accelerometer driver which uses I²C via
2//! [embedded-hal]. This driver implements the [`Accelerometer`][acc-trait]
3//! and [`RawAccelerometer`][raw-trait] traits from the [accelerometer] crate.
4//!
5//! [embedded-hal]: https://docs.rs/embedded-hal
6//! [accelerometer]: https://docs.rs/accelerometer
7//! [acc-trait]: https://docs.rs/accelerometer/latest/accelerometer/trait.Accelerometer.html
8//! [raw-trait]: https://docs.rs/accelerometer/latest/accelerometer/trait.RawAccelerometer.html
9//!
10
11#![no_std]
12
13pub use accelerometer;
14use accelerometer::error::Error as AccelerometerError;
15use accelerometer::vector::{F32x3, I16x3};
16use accelerometer::{Accelerometer, RawAccelerometer};
17use embedded_hal::i2c::I2c;
18
19pub mod config;
20pub mod register;
21use config::*;
22use register::*;
23use register::{DataRate, Mode, Range, Register, SlaveAddr};
24
25/// Accelerometer errors, generic around another error type `E` representing
26/// an (optional) cause of this error.
27#[derive(Debug)]
28pub enum Error<BusError, PinError> {
29    /// I²C bus error
30    Bus(BusError),
31    Pin(PinError),
32
33    /// Invalid Axis and direction
34    InvalidAxis,
35
36    /// Invalid data rate selection
37    InvalidDataRate,
38
39    /// Invalid acceleration range selection
40    InvalidRange,
41
42    /// Invalid operating mode selection
43    InvalidMode,
44
45    /// Attempted to write to a read-only register
46    WriteToReadOnly,
47
48    /// Invalid address provided
49    WrongAddress,
50}
51
52/// `KXTJ3-1057` driver.
53pub struct Kxtj3<I2C> {
54    /// Underlying I²C device
55    i2c: I2C,
56
57    /// Current I²C slave address
58    address: u8,
59}
60
61impl<I2C, E> Kxtj3<I2C>
62where
63    I2C: I2c<Error = E>,
64{
65    /// Create a new KXTJ3-1057 driver from the given I2C peripheral.
66    /// Default is Hz_400.
67    /// An example using the [esp_idf_hal](https://esp-rs.github.io/esp-idf-hal/esp_idf_hal):
68    ///     
69    ///     use esp_idf_svc::hal::{delay::FreeRtos, i2c::*, prelude::Peripherals, units::Hertz};
70    ///     use kxtj3_1057::{register::SlaveAddr, Kxtj3};
71    ///           
72    ///     let peripherals = Peripherals::take().unwrap();
73    ///     let i2c = peripherals.i2c0;
74    ///     let sda = peripherals.pins.gpio10;
75    ///     let scl = peripherals.pins.gpio8;
76    ///     let config = I2cConfig::new().baudrate(Hertz(400_000)).scl_enable_pullup(true).sda_enable_pullup(true);
77    ///     let i2c = I2cDriver::new(i2c, sda, scl, &config).unwrap();
78    ///     let mut kxtj3 = Kxtj3::new(i2c, SlaveAddr::Default).unwrap();
79
80    pub fn new(i2c: I2C, address: SlaveAddr) -> Result<Self, Error<E, core::convert::Infallible>> {
81        Self::new_with_config(i2c, address, Configuration::default())
82    }
83
84    pub fn new_with_config(
85        i2c: I2C,
86        address: SlaveAddr,
87        config: Configuration,
88    ) -> Result<Self, Error<E, core::convert::Infallible>> {
89        let mut kxtj3 = Kxtj3 {
90            i2c,
91            address: address.addr(),
92        };
93        kxtj3.configure(config)?;
94
95        Ok(kxtj3)
96    }
97
98    /// Configures the device
99    pub fn configure(
100        &mut self,
101        conf: Configuration,
102    ) -> Result<(), Error<E, core::convert::Infallible>> {
103        if self.get_device_id()? != DEVICE_ID {
104            return Err(Error::WrongAddress);
105        }
106        self.enable_standby_mode()?;
107        self.set_mode(conf.mode)?;
108        self.set_range(conf.range)?;
109        self.set_datarate(conf.datarate)?;
110
111        if conf.enable_new_acceleration_interrupt {
112            self.enable_new_accelration_interrupt()?;
113        }
114        if let Some(md_conf) = conf.motion_detection {
115            self.enable_motion_detection()?;
116            self.set_motion_detection_datarate(md_conf.datarate)?;
117            self.set_motion_detection_latch_mode(md_conf.latch_mode)?;
118            self.set_motion_detection_na_counter(md_conf.non_activity_counter)?;
119            self.set_motion_detection_wakeup_counter(md_conf.wakeup_counter)?;
120            self.set_motion_detection_threshold(md_conf.wakeup_threshold)?;
121            self.enable_motion_detection_axes(
122                md_conf.enable_x_negative,
123                md_conf.enable_x_positive,
124                md_conf.enable_y_negative,
125                md_conf.enable_y_positive,
126                md_conf.enable_z_negative,
127                md_conf.enable_z_positive,
128            )?;
129            if let Some(ip_conf) = md_conf.interrupt_pin {
130                self.set_motion_detection_interrupt_pin_polarity(ip_conf.polarity)?;
131                self.set_motion_detection_interrupt_pin_response(ip_conf.response)?;
132            }
133        }
134        self.disable_standby_mode()
135    }
136
137    /// Writes a byte to the given register.
138    fn write_register(
139        &mut self,
140        register: Register,
141        value: u8,
142    ) -> Result<(), Error<E, core::convert::Infallible>> {
143        if register.read_only() {
144            return Err(Error::WriteToReadOnly);
145        }
146
147        self.i2c
148            .write(self.address, &[register.addr(), value])
149            .map_err(Error::Bus)
150    }
151
152    /// Reads a byte from the given register.
153    fn read_register(
154        &mut self,
155        register: Register,
156    ) -> Result<u8, Error<E, core::convert::Infallible>> {
157        let mut data = [0];
158        self.i2c
159            .write_read(self.address, &[register.addr()], &mut data)
160            .map_err(Error::Bus)
161            .and(Ok(data[0]))
162    }
163
164    /// `WHO_AM_I` register.
165    pub fn get_device_id(&mut self) -> Result<u8, Error<E, core::convert::Infallible>> {
166        self.read_register(Register::WHOAMI)
167    }
168
169    /// Enable stand-by mode .
170    /// `CTRL_REG1`: `PC1` bit.
171    pub fn enable_standby_mode(&mut self) -> Result<(), Error<E, core::convert::Infallible>> {
172        self.register_clear_bits(Register::CTRL1, PC1_EN)
173    }
174
175    /// Disable stand-by mode .
176    /// `CTRL_REG1`: `PC1` bit.
177    pub fn disable_standby_mode(&mut self) -> Result<(), Error<E, core::convert::Infallible>> {
178        self.register_set_bits(Register::CTRL1, PC1_EN)
179    }
180
181    /// Controls the operating mode of the KXTJ3 .
182    /// `CTRL_REG1`: `RES` bit.
183    /// Before using this function, the device must be in standby mode.
184    pub fn set_mode(&mut self, mode: Mode) -> Result<(), Error<E, core::convert::Infallible>> {
185        match mode {
186            Mode::LowPower => {
187                self.register_clear_bits(Register::CTRL1, RES_EN)?;
188            }
189            Mode::HighResolution => {
190                self.register_set_bits(Register::CTRL1, RES_EN)?;
191            }
192            _ => {
193                return Err(Error::InvalidMode);
194            }
195        }
196
197        Ok(())
198    }
199
200    /// Reads the current operating mode.
201    pub fn get_mode(&mut self) -> Result<Mode, Error<E, core::convert::Infallible>> {
202        let ctrl1 = self.read_register(Register::CTRL1)?;
203
204        let is_pc1_set = (ctrl1 >> 7) & 0x01 != 0;
205        let is_res_set = (ctrl1 >> 6) & 0x01 != 0;
206
207        let mode = match (is_pc1_set, is_res_set) {
208            (false, _) => Mode::Standby,
209            (true, false) => Mode::LowPower,
210            (true, true) => Mode::HighResolution,
211        };
212
213        Ok(mode)
214    }
215
216    /// Data rate selection.
217    ///
218    /// Before using this function, the device must be in standby mode.
219    pub fn set_datarate(
220        &mut self,
221        datarate: DataRate,
222    ) -> Result<(), Error<E, core::convert::Infallible>> {
223        self.modify_register(Register::DATA_CTRL, |mut data_ctrl| {
224            // Mask off highest 4 bits
225            data_ctrl &= !ODR_MASK;
226            // Write in lowest 4 bits
227            data_ctrl |= datarate.bits();
228
229            data_ctrl
230        })
231    }
232
233    /// Reads the current data selection rate.
234    pub fn get_datarate(&mut self) -> Result<DataRate, Error<E, core::convert::Infallible>> {
235        let data_ctrl = self.read_register(Register::DATA_CTRL)?;
236        let odr = data_ctrl & 0x0F;
237
238        DataRate::try_from(odr).map_err(|_| Error::InvalidDataRate)
239    }
240
241    /// Sets the acceleration Range.
242    ///
243    /// Before using this function, the device must be in standby mode.
244    pub fn set_range(&mut self, range: Range) -> Result<(), Error<E, core::convert::Infallible>> {
245        self.modify_register(Register::CTRL1, |mut ctrl1| {
246            ctrl1 &= !GSEL_MASK;
247            ctrl1 |= range.bits() << 2;
248            ctrl1
249        })
250    }
251
252    /// Reads the acceleration Range
253    pub fn get_range(&mut self) -> Result<Range, Error<E, core::convert::Infallible>> {
254        let ctrl1 = self.read_register(Register::CTRL1)?;
255        let gsel = (ctrl1 >> 2) & 0x07;
256
257        Range::try_from(gsel).map_err(|_| Error::InvalidRange)
258    }
259
260    /// Reads from the registers for each of the 3 axes.
261    fn read_accel_bytes(&mut self) -> Result<[u8; 6], Error<E, core::convert::Infallible>> {
262        let mut data = [0u8; 6];
263
264        self.i2c
265            .write_read(self.address, &[Register::XOUT_L.addr() | 0x80], &mut data)
266            .map_err(Error::Bus)
267            .and(Ok(data))
268    }
269
270    /// Enables the reporting of the availability of new acceleration data as an interrupt.
271    ///
272    /// Before using this function, the device must be in standby mode.
273    pub fn enable_new_accelration_interrupt(
274        &mut self,
275    ) -> Result<(), Error<E, core::convert::Infallible>> {
276        self.register_set_bits(Register::CTRL1, DRDYE_EN)
277    }
278
279    /// Enables the Wake-Up (motion detect) function.
280    ///
281    /// Before using this function, the device must be in standby mode.
282    pub fn enable_motion_detection(&mut self) -> Result<(), Error<E, core::convert::Infallible>> {
283        self.register_set_bits(Register::CTRL1, WUFE_EN)
284    }
285
286    /// Enables the physical interrupt pin (INT).
287    ///
288    /// Before using this function, the device must be in standby mode.
289    pub fn enable_motion_detection_physical_interrupt_pin(
290        &mut self,
291    ) -> Result<(), Error<E, core::convert::Infallible>> {
292        self.register_set_bits(Register::INT_CTRL1, IEN_EN)
293    }
294
295    /// Sets the polarity of the physical interrupt pin
296    ///
297    /// Before using this function, the device must be in standby mode.
298    pub fn set_motion_detection_interrupt_pin_polarity(
299        &mut self,
300        polarity: InterruptPinPolarity,
301    ) -> Result<(), Error<E, core::convert::Infallible>> {
302        match polarity {
303            InterruptPinPolarity::ActiveHigh => self.register_set_bits(Register::INT_CTRL1, IEA_EN),
304            InterruptPinPolarity::ActiveLow => {
305                self.register_clear_bits(Register::INT_CTRL1, IEA_EN)
306            }
307        }
308    }
309
310    /// Sets the response of the physical interrupt pin.
311    ///
312    /// Before using this function, the device must be in standby mode.
313    pub fn set_motion_detection_interrupt_pin_response(
314        &mut self,
315        response: InterruptPinResponse,
316    ) -> Result<(), Error<E, core::convert::Infallible>> {
317        match response {
318            InterruptPinResponse::Latched => self.register_clear_bits(Register::INT_CTRL1, IEL_EN),
319            InterruptPinResponse::Pulsed => self.register_set_bits(Register::INT_CTRL1, IEL_EN),
320        }
321    }
322
323    /// Sets the Output Data Rate for the motion detection function
324    ///
325    /// Before using this function, the device must be in standby mode.
326    pub fn set_motion_detection_datarate(
327        &mut self,
328        datarate: MotionDetectionDataRate,
329    ) -> Result<(), Error<E, core::convert::Infallible>> {
330        self.modify_register(Register::CTRL2, |mut ctrl2| {
331            ctrl2 &= !ODRW_MASK;
332            ctrl2 |= datarate.bits();
333            ctrl2
334        })
335    }
336
337    /// Reads the current data selection rate the motion detection function.
338    pub fn get_motion_detection_datarate(
339        &mut self,
340    ) -> Result<MotionDetectionDataRate, Error<E, core::convert::Infallible>> {
341        let data_ctrl = self.read_register(Register::CTRL2)?;
342        let odr = data_ctrl & 0x07;
343        MotionDetectionDataRate::try_from(odr).map_err(|_| Error::InvalidDataRate)
344    }
345
346    /// Sets the motion detection latch mode.
347    ///
348    /// Before using this function, the device must be in standby mode.
349    pub fn set_motion_detection_latch_mode(
350        &mut self,
351        latch_mode: MotionDetectionLatchMode,
352    ) -> Result<(), Error<E, core::convert::Infallible>> {
353        self.modify_register(Register::INT_CTRL2, |mut int_ctrl2| {
354            int_ctrl2 &= !ULMODE_EN;
355            int_ctrl2 |= latch_mode.bits() << 7;
356            int_ctrl2
357        })
358    }
359
360    /// Sets the time motion must be present before a wake-up interrupt is set.
361    ///
362    /// `WAKEUP_COUNTER (counts) = Wake-Up Delay Time (sec) x Wake-Up Function ODR (Hz)`
363    ///
364    /// Before using this function, the device must be in standby mode.
365    pub fn set_motion_detection_wakeup_counter(
366        &mut self,
367        wakeup_counter: u8,
368    ) -> Result<(), Error<E, core::convert::Infallible>> {
369        self.write_register(Register::WAKEUP_COUNTER, wakeup_counter)
370    }
371
372    /// Sets the non-activity time required before another wake-up interrupt can be set.
373    ///
374    /// `NA_COUNTER (counts) = Non-Activity Time (sec) x Wake-Up Function ODR (Hz)`
375    ///
376    /// Before using this function, the device must be in standby mode.
377    pub fn set_motion_detection_na_counter(
378        &mut self,
379        na_counter: u8,
380    ) -> Result<(), Error<E, core::convert::Infallible>> {
381        self.write_register(Register::NA_COUNTER, na_counter)
382    }
383
384    /// Sets the threshold for motion detection interrupt is set.
385    ///
386    /// `WAKEUP_THRESHOLD (counts) = Desired Threshold (g) x 256 (counts/g)`
387    ///
388    /// Before using this function, the device must be in standby mode.
389    pub fn set_motion_detection_threshold(
390        &mut self,
391        desired_threshold: u8,
392    ) -> Result<(), Error<E, core::convert::Infallible>> {
393        let upper_8_bits = (desired_threshold >> 4) as u8;
394        let lower_8_bits = (desired_threshold << 4) as u8;
395        self.write_register(Register::WAKEUP_THRESHOLD_H, upper_8_bits)?;
396        self.write_register(Register::WAKEUP_THRESHOLD_L, lower_8_bits)
397    }
398
399    /// Reports the axis and direction of detected motion.
400    pub fn get_motion_detection_axis(
401        &mut self,
402    ) -> Result<MotionDetectionAxis, Error<E, core::convert::Infallible>> {
403        let int_src2 = self.read_register(Register::INT_SOURCE2)?;
404        MotionDetectionAxis::try_from(int_src2).map_err(|_| Error::InvalidAxis)
405    }
406    ///Indicates Wake-up has occurred or not.
407    pub fn is_motion_detected(&mut self) -> Result<bool, Error<E, core::convert::Infallible>> {
408        let int_src1 = self.read_register(Register::INT_SOURCE1)?;
409        let wufs = (int_src1 >> 1) & 0x01 != 0;
410        Ok(wufs)
411    }
412
413    /// Indicates that new acceleration data (0x06 to 0x0B) is available or not .
414    pub fn is_acceleration_data_ready(
415        &mut self,
416    ) -> Result<bool, Error<E, core::convert::Infallible>> {
417        let int_src1 = self.read_register(Register::INT_SOURCE1)?;
418        let drdy = (int_src1 >> 4) & 0x01 != 0;
419        Ok(drdy)
420    }
421
422    /// Sets which axes and directions of detected motion can cause an interrupt.
423    ///
424    /// Before using this function, the device must be in standby mode.
425    pub fn enable_motion_detection_axes(
426        &mut self,
427        x_negative: bool,
428        x_positive: bool,
429        y_negative: bool,
430        y_positive: bool,
431        z_negative: bool,
432        z_positive: bool,
433    ) -> Result<(), Error<E, core::convert::Infallible>> {
434        self.modify_register(Register::INT_CTRL2, |mut int_ctrl2| {
435            int_ctrl2 &= !WUE_MASK;
436            int_ctrl2 |= if x_negative {
437                MotionDetectionAxis::X_Negative.bits()
438            } else {
439                0
440            };
441            int_ctrl2 |= if x_positive {
442                MotionDetectionAxis::X_Positive.bits()
443            } else {
444                0
445            };
446            int_ctrl2 |= if y_negative {
447                MotionDetectionAxis::Y_Negative.bits()
448            } else {
449                0
450            };
451            int_ctrl2 |= if y_positive {
452                MotionDetectionAxis::Y_Positive.bits()
453            } else {
454                0
455            };
456            int_ctrl2 |= if z_negative {
457                MotionDetectionAxis::Z_Negative.bits()
458            } else {
459                0
460            };
461            int_ctrl2 |= if z_positive {
462                MotionDetectionAxis::Z_Positive.bits()
463            } else {
464                0
465            };
466            int_ctrl2
467        })
468    }
469
470    /// Clears Latched interrupt source information (INT_SOURCE1 and INT_SOURCE2).
471    /// Changes physical interrupt latched pin (INT) to inactive state.
472    pub fn clear_motion_detection_lathced_info(
473        &mut self,
474    ) -> Result<(), Error<E, core::convert::Infallible>> {
475        self.read_register(Register::INT_REL)?;
476        Ok(())
477    }
478
479    /// Modify a register's value. Read the current value of the register,
480    /// update the value with the provided function, and set the register to
481    /// the return value.
482    fn modify_register<F>(
483        &mut self,
484        register: Register,
485        f: F,
486    ) -> Result<(), Error<E, core::convert::Infallible>>
487    where
488        F: FnOnce(u8) -> u8,
489    {
490        let value = self.read_register(register)?;
491
492        self.write_register(register, f(value))
493    }
494
495    /// Clear the given bits in the given register. For example:
496    ///
497    ///     kxtj3.register_clear_bits(0b0110)
498    ///
499    /// This call clears (sets to 0) the bits at index 1 and 2. Other bits of the register are not touched.
500    pub fn register_clear_bits(
501        &mut self,
502        reg: Register,
503        bits: u8,
504    ) -> Result<(), Error<E, core::convert::Infallible>> {
505        self.modify_register(reg, |v| v & !bits)
506    }
507
508    /// Set the given bits in the given register. For example:
509    ///
510    ///     kxtj3.register_set_bits(0b0110)
511    ///
512    /// This call sets to 1 the bits at index 1 and 2. Other bits of the register are not touched.
513    pub fn register_set_bits(
514        &mut self,
515        reg: Register,
516        bits: u8,
517    ) -> Result<(), Error<E, core::convert::Infallible>> {
518        self.modify_register(reg, |v| v | bits)
519    }
520}
521
522impl<I2C, E> RawAccelerometer<I16x3> for Kxtj3<I2C>
523where
524    I2C: I2c<Error = E>,
525    E: core::fmt::Debug,
526{
527    type Error = Error<E, core::convert::Infallible>;
528
529    fn accel_raw(&mut self) -> Result<I16x3, AccelerometerError<Self::Error>> {
530        let accel_bytes = self.read_accel_bytes()?;
531
532        let x = i16::from_le_bytes(accel_bytes[0..2].try_into().unwrap());
533        let y = i16::from_le_bytes(accel_bytes[2..4].try_into().unwrap());
534        let z = i16::from_le_bytes(accel_bytes[4..6].try_into().unwrap());
535
536        Ok(I16x3::new(x, y, z))
537    }
538}
539
540impl<I2C, E> Accelerometer for Kxtj3<I2C>
541where
542    I2C: I2c<Error = E>,
543    E: core::fmt::Debug,
544{
545    type Error = Error<E, core::convert::Infallible>;
546
547    /// Get normalized ±g reading from the accelerometer.
548    fn accel_norm(&mut self) -> Result<F32x3, AccelerometerError<Self::Error>> {
549        let mode = self.get_mode()?;
550        let range = self.get_range()?;
551
552        // See "Mechanical Specifications" in the datasheet to find the values below.
553
554        // Depending on which Mode we are operating in, the data has different
555        // resolution. Using this knowledge, we determine how many bits the
556        // data needs to be shifted. This is necessary because the raw data
557        // is in left-justified two's complement and we would like for it to be
558        // right-justified instead.
559
560        let (scale, shift) = match (mode, range) {
561            // High Resolution mode - 14-bit data output
562            (Mode::HighResolution, Range::G8_14Bit) => (0.001, 2),
563            (Mode::HighResolution, Range::G16_14Bit) => (0.002, 2),
564            // High Resolution mode-12 bit data output
565            (Mode::HighResolution, Range::G2) => (0.001, 4),
566            (Mode::HighResolution, Range::G4) => (0.002, 4),
567            (Mode::HighResolution, Range::G8) => (0.004, 4),
568            (Mode::HighResolution, Range::G16_1)
569            | (Mode::HighResolution, Range::G16_2)
570            | (Mode::HighResolution, Range::G16_3) => (0.008, 4),
571
572            // Low power mode
573            (Mode::LowPower, Range::G2) => (0.015, 8),
574            (Mode::LowPower, Range::G4) => (0.031, 8),
575            (Mode::LowPower, Range::G8) => (0.062, 8),
576            (Mode::LowPower, Range::G16_1)
577            | (Mode::LowPower, Range::G16_2)
578            | (Mode::LowPower, Range::G16_3) => (0.125, 8),
579
580            _ => (0.0, 0),
581        };
582
583        let acc_raw = self.accel_raw()?;
584        let x = (acc_raw.x >> shift) as f32 * scale;
585        let y = (acc_raw.y >> shift) as f32 * scale;
586        let z = (acc_raw.z >> shift) as f32 * scale;
587
588        Ok(F32x3::new(x, y, z))
589    }
590
591    /// Get the sample rate of the accelerometer data.
592    fn sample_rate(&mut self) -> Result<f32, AccelerometerError<Self::Error>> {
593        Ok(self.get_datarate()?.sample_rate())
594    }
595}
kxtj3_1057/lib.rs

kxtj3_1057/
lib.rs
