edrv_l3g4200d/
lib.rs

1//! Driver for L3G4200D.
2
3#![no_std]
4
5pub const ADDRESS: u8 = 0x69;
6
7pub mod regs {
8    pub const WHO_AM_I: u8 = 0x0F;
9
10    pub const CTRL_REG1: u8 = 0x20;
11    pub const CTRL_REG2: u8 = 0x21;
12    pub const CTRL_REG3: u8 = 0x22;
13    pub const CTRL_REG4: u8 = 0x23;
14    pub const CTRL_REG5: u8 = 0x24;
15    pub const REFERENCE: u8 = 0x25;
16    pub const OUT_TEMP: u8 = 0x26;
17    pub const STATUS_REG: u8 = 0x27;
18
19    pub const OUT_X_L: u8 = 0x28;
20    pub const OUT_X_H: u8 = 0x29;
21    pub const OUT_Y_L: u8 = 0x2A;
22    pub const OUT_Y_H: u8 = 0x2B;
23    pub const OUT_Z_L: u8 = 0x2C;
24    pub const OUT_Z_H: u8 = 0x2D;
25
26    pub const FIFO_CTRL_REG: u8 = 0x2E;
27    pub const FIFO_SRC_REG: u8 = 0x2F;
28
29    pub const INT1_CFG: u8 = 0x30;
30    pub const INT1_SRC: u8 = 0x31;
31    pub const INT1_THS_XH: u8 = 0x32;
32    pub const INT1_THS_XL: u8 = 0x33;
33    pub const INT1_THS_YH: u8 = 0x34;
34    pub const INT1_THS_YL: u8 = 0x35;
35    pub const INT1_THS_ZH: u8 = 0x36;
36    pub const INT1_THS_ZL: u8 = 0x37;
37    pub const INT1_DURATION: u8 = 0x38;
38}
39
40#[derive(Debug, Clone, Copy, PartialEq, Eq)]
41#[repr(u8)]
42pub enum Scale {
43    Dps2000 = 0b10,
44    Dps500 = 0b01,
45    Dps250 = 0b00,
46}
47
48#[derive(Debug, Clone, Copy, PartialEq, Eq)]
49#[repr(u8)]
50pub enum DataRate {
51    Hz800Bw110 = 0b1111,
52    Hz800Bw50 = 0b1110,
53    Hz800Bw35 = 0b1101,
54    Hz800Bw30 = 0b1100,
55    Hz400Bw110 = 0b1011,
56    Hz400Bw50 = 0b1010,
57    Hz400Bw25 = 0b1001,
58    Hz400Bw20 = 0b1000,
59    Hz200Bw70 = 0b0111,
60    Hz200Bw50 = 0b0110,
61    Hz200Bw25 = 0b0101,
62    Hz200Bw12_5 = 0b0100,
63    Hz100Bw25 = 0b0001,
64    Hz100Bw12_5 = 0b0000,
65}
66
67#[derive(Debug)]
68pub enum Error<IE> {
69    Bus(IE),
70    InvalidDevice,
71}
72
73impl<E> From<E> for Error<E> {
74    fn from(e: E) -> Self {
75        Error::Bus(e)
76    }
77}
78
79pub struct Config {
80    pub scale: Scale,
81    pub data_rate: DataRate,
82}
83
84impl Default for Config {
85    fn default() -> Self {
86        Config {
87            scale: Scale::Dps2000,
88            data_rate: DataRate::Hz400Bw50,
89        }
90    }
91}
92
93pub struct L3G4200D<I2C> {
94    i2c: I2C,
95    addr: u8,
96    dps_per_digit: f32,
97}
98
99impl<I2C> L3G4200D<I2C>
100where
101    I2C: embedded_hal::i2c::I2c,
102{
103    pub fn new(i2c: I2C, addr: u8) -> Self {
104        Self {
105            i2c,
106            addr,
107            dps_per_digit: 0.00875,
108        }
109    }
110
111    pub fn new_primary(i2c: I2C) -> Self {
112        Self::new(i2c, ADDRESS)
113    }
114
115    pub fn init(&mut self, config: Config) -> Result<(), Error<I2C::Error>> {
116        if self.read_reg(regs::WHO_AM_I)? != 0xD3 {
117            return Err(Error::InvalidDevice);
118        }
119
120        // Enable all axis and setup normal mode + Output Data Range & Bandwidth
121        let mut reg1 = 0x0F; // Enable all axis and setup normal mode
122        reg1 |= (config.data_rate as u8) << 4; // Set output data rate & bandwidth
123        self.write_reg(regs::CTRL_REG1, reg1)?;
124
125        // Disable high pass filter
126        self.write_reg(regs::CTRL_REG2, 0x00)?;
127
128        // Generate data ready interrupt on INT2
129        self.write_reg(regs::CTRL_REG3, 0x08)?;
130
131        // Set full scale selection in continuous mode
132        self.write_reg(regs::CTRL_REG4, (config.scale as u8) << 4)?;
133
134        // Set dpsPerDigit based on scale
135        self.dps_per_digit = match config.scale {
136            Scale::Dps250 => 0.00875,
137            Scale::Dps500 => 0.0175,
138            Scale::Dps2000 => 0.07,
139        };
140
141        // Boot in normal mode, disable FIFO, HPF disabled
142        self.write_reg(regs::CTRL_REG5, 0x00)?;
143
144        Ok(())
145    }
146
147    pub fn read_raw(&mut self) -> Result<(i16, i16, i16), Error<I2C::Error>> {
148        let mut buf = [0u8; 6];
149
150        // Read 6 bytes starting from OUT_X_L register (0x28 | 0x80 for auto-increment)
151        self.i2c
152            .write_read(self.addr, &[regs::OUT_X_L | 0x80], &mut buf)?;
153
154        // Combine high and low bytes into 16-bit integers
155        let x = i16::from_le_bytes([buf[0], buf[1]]);
156        let y = i16::from_le_bytes([buf[2], buf[3]]);
157        let z = i16::from_le_bytes([buf[4], buf[5]]);
158
159        Ok((x, y, z))
160    }
161
162    pub fn read_normalized(&mut self) -> Result<(i16, i16, i16), Error<I2C::Error>> {
163        let (x, y, z) = self.read_raw()?;
164
165        // Apply normalization using dps_per_digit
166        let x_norm = (x as f32 * self.dps_per_digit) as i16;
167        let y_norm = (y as f32 * self.dps_per_digit) as i16;
168        let z_norm = (z as f32 * self.dps_per_digit) as i16;
169
170        Ok((x_norm, y_norm, z_norm))
171    }
172
173    pub fn read_reg(&mut self, reg: u8) -> Result<u8, I2C::Error> {
174        let mut buf = [0];
175        self.i2c.write_read(self.addr, &[reg], &mut buf)?;
176        Ok(buf[0])
177    }
178
179    // Add this new method to write to registers
180    pub fn write_reg(&mut self, reg: u8, value: u8) -> Result<(), I2C::Error> {
181        self.i2c.write(self.addr, &[reg, value])
182    }
183}
edrv_l3g4200d/lib.rs

edrv_l3g4200d/
lib.rs
