tof_control/device/
lis3mdltr.rs

1#![allow(unused)]
2use crate::constant::*;
3
4use i2cdev::core::*;
5use i2cdev::linux::{LinuxI2CDevice, LinuxI2CError};
6
7// LIS3MDLTR Register
8const WHO_AM_I: u16 = 0x0F;
9const CTRL_REG1: u16 = 0x20;
10const CTRL_REG2: u16 = 0x21;
11const CTRL_REG3: u16 = 0x22;
12const CTRL_REG4: u16 = 0x23;
13const CTRL_REG5: u16 = 0x24;
14const STATUS: u16 = 0x27;
15const OUT_X_L: u16 = 0x28;
16const OUT_X_H: u16 = 0x29;
17const OUT_Y_L: u16 = 0x2A;
18const OUT_Y_H: u16 = 0x2B;
19const OUT_Z_L: u16 = 0x2C;
20const OUT_Z_H: u16 = 0x2D;
21const TEMP_OUT_L: u16 = 0x2E;
22const TEMP_OUT_H: u16 = 0x2F;
23const INT_CFG: u16 = 0x30;
24const INT_SRC: u16 = 0x31;
25const INT_THS_L: u16 = 0x32;
26const INT_THS_H: u16 = 0x33;
27// CTRL_REG1 Configuration
28const TEMP_EN: u16 = 0x80; // Enable temperature sensor
29const TEMP_DI: u16 = 0x00; // Disable temperature sensor
30const OM_LPM: u16 = 0x00; // Low-power mode
31const OM_MPM: u16 = 0x20; // Medium-performance mode
32const OM_HPM: u16 = 0x40; // High-performance mode
33const OM_UHPM: u16 = 0x60; // Ultra-high-performance mode
34const DO_0_625: u16 = 0x00; // Output data rate: 0.625Hz
35const DO_1_25: u16 = 0x04; // Output data rate: 1.25Hz
36const DO_2_5: u16 = 0x08; // Output data rate: 2.5Hz
37const DO_5: u16 = 0x0C; // Output data rate: 5Hz
38const DO_10: u16 = 0x10; // Output data rate: 10Hz
39const DO_20: u16 = 0x14; // Output data rate: 20Hz
40const DO_40: u16 = 0x18; // Output data rate: 40Hz
41const DO_80: u16 = 0x1C; // Output data rate: 80Hz
42const FAST_ODR_DI: u16 = 0x00; // Fast_ODR disabled
43const FAST_ODR_EN: u16 = 0x02; // Fast_ODR enabled
44const ST_DI: u16 = 0x00; // Self-test disabled
45const ST_EN: u16 = 0x01; // Self-test enabled
46                         // CTRL_REG2 Configuration
47const FS_4: u16 = 0x00; // Full-scale: ±4 gauss
48const FS_8: u16 = 0x20; // Full-scale: ±8 gauss
49const FS_12: u16 = 0x40; // Full-scale: ±12 gauss
50const FS_16: u16 = 0x60; // Full-scale: ±16 gauss
51const REBOOT_NM: u16 = 0x00; // Reboot memory content: normal mode
52const REBOOT_RMC: u16 = 0x08; // Reboot memory content: reboot memory content
53const SOFT_RST: u16 = 0x04; // Reset operation
54                            // CTRL_REG3 Configuration
55const LP: u16 = 0x20; // Low-power mode configuration
56const SPI_4: u16 = 0x00; // SPI serial interface mode selection: 4-wire interface
57const SPI_3: u16 = 0x04; // SPI serial interface mode selection: 3-wire interface
58const MD_CCM: u16 = 0x00; // Continuous-conversion mode
59const MD_SCM: u16 = 0x01; // Single-conversion mode
60const MD_PDM: u16 = 0x02; // Power-down mode
61const MD_PDM_2: u16 = 0x03; // Power-down mode 2
62                            // CTRL_REG4 Configuration
63const OMZ_LPM: u16 = 0x00; // Operating mode for Z-axis: low-power mode
64const OMZ_MPM: u16 = 0x04; // Operating mode for Z-axis: medium-performance mode
65const OMZ_HPM: u16 = 0x08; // Operating mode for Z-axis: high-performance mode
66const OMZ_UHPM: u16 = 0x0C; // Operating mode for Z-axis: ultra-high-performance mode
67const BLE_LSB: u16 = 0x00; // Big/Little Endian data selection: data LSb at lower address
68const BLE_MSB: u16 = 0x02; // Big/Little Endian data selection: data MSb at lower address
69                           // CTRL_REG5 Configuration
70const FAST_READ_DI: u16 = 0x00; // FAST_READ disabled
71const FAST_READ_EN: u16 = 0x80; // FAST_READ enabled
72const BDU_CU: u16 = 0x00; // Block data update for magnetic data: continuous update
73const BDU_OR: u16 = 0x40; // Block data update for magnetic data: output registers not updated until MSb and LSb have been read)
74                          // Sensitivity
75const SENSITIVITY_4: u16 = 6842; // Magnetic sensor sensitivity at FS=±4 gauss [LSB/gauss]
76const SENSITIVITY_8: u16 = 3421; // Magnetic sensor sensitivity at FS=±8 gauss [LSB/gauss]
77const SENSITIVITY_12: u16 = 2281; // Magnetic sensor sensitivity at FS=±12 gauss [LSB/gauss]
78const SENSITIVITY_16: u16 = 1711; // Magnetic sensor sensitivity at FS=±16 gauss [LSB/gauss]
79const SENSITIVITY_TEMP: u16 = 8; // Temperature sensitivity [LSB/°C] (@ Vdd = 2.5 V, T = 25 °C)
80
81pub struct LIS3MDLTR {
82    bus: u8,
83    address: u16,
84}
85
86impl LIS3MDLTR {
87    pub fn new(bus: u8, address: u16) -> Self {
88        Self { bus, address }
89    }
90    pub fn configure(&self) -> Result<(), LinuxI2CError> {
91        let mut dev = LinuxI2CDevice::new(&format!("/dev/i2c-{}", self.bus), self.address)?;
92
93        self.configure_2(&mut dev, true)?;
94        self.configure_1(&mut dev)?;
95        self.configure_2(&mut dev, false)?;
96        self.configure_3(&mut dev)?;
97        self.configure_4(&mut dev)?;
98        self.configure_5(&mut dev)?;
99
100        Ok(())
101    }
102    fn configure_1(&self, dev: &mut LinuxI2CDevice) -> Result<(), LinuxI2CError> {
103        let config = TEMP_EN | OM_MPM | DO_10 | FAST_ODR_DI | ST_DI;
104        dev.smbus_write_byte_data(CTRL_REG1 as u8, config as u8)?;
105
106        Ok(())
107    }
108    fn configure_2(&self, dev: &mut LinuxI2CDevice, reset: bool) -> Result<(), LinuxI2CError> {
109        let mut config: u16;
110        if reset {
111            config = SOFT_RST;
112        } else {
113            config = FS_4 | REBOOT_NM;
114        }
115        dev.smbus_write_byte_data(CTRL_REG2 as u8, config as u8)?;
116
117        Ok(())
118    }
119    fn configure_3(&self, dev: &mut LinuxI2CDevice) -> Result<(), LinuxI2CError> {
120        let config = MD_CCM;
121        dev.smbus_write_byte_data(CTRL_REG3 as u8, config as u8)?;
122
123        Ok(())
124    }
125    fn configure_4(&self, dev: &mut LinuxI2CDevice) -> Result<(), LinuxI2CError> {
126        let config = OMZ_MPM | BLE_LSB;
127        dev.smbus_write_byte_data(CTRL_REG4 as u8, config as u8)?;
128
129        Ok(())
130    }
131    fn configure_5(&self, dev: &mut LinuxI2CDevice) -> Result<(), LinuxI2CError> {
132        let config = FAST_READ_DI | BDU_CU;
133        dev.smbus_write_byte_data(CTRL_REG5 as u8, config as u8)?;
134
135        Ok(())
136    }
137    fn read_mag_x(&self) -> Result<f32, LinuxI2CError> {
138        let mut dev = LinuxI2CDevice::new(&format!("/dev/i2c-{}", self.bus), self.address)?;
139        let out_x_h = dev.smbus_read_byte_data(OUT_X_H as u8)?;
140        let out_x_l = dev.smbus_read_byte_data(OUT_X_L as u8)?;
141        let out_x_adc = (((out_x_h as u16) << 8) | out_x_l as u16) & 0xFFFF;
142        let mag_x = self.adc_to_gauss(out_x_adc, SENSITIVITY_4);
143
144        Ok(mag_x)
145    }
146    fn read_mag_y(&self) -> Result<f32, LinuxI2CError> {
147        let mut dev = LinuxI2CDevice::new(&format!("/dev/i2c-{}", self.bus), self.address)?;
148        let out_y_h = dev.smbus_read_byte_data(OUT_Y_H as u8)?;
149        let out_y_l = dev.smbus_read_byte_data(OUT_Y_L as u8)?;
150        let out_y_adc = (((out_y_h as u16) << 8) | out_y_l as u16) & 0xFFFF;
151        let mag_y = self.adc_to_gauss(out_y_adc, SENSITIVITY_4);
152
153        Ok(mag_y)
154    }
155    fn read_mag_z(&self) -> Result<f32, LinuxI2CError> {
156        let mut dev = LinuxI2CDevice::new(&format!("/dev/i2c-{}", self.bus), self.address)?;
157        let out_z_h = dev.smbus_read_byte_data(OUT_Z_H as u8)?;
158        let out_z_l = dev.smbus_read_byte_data(OUT_Z_L as u8)?;
159        let out_z_adc = (((out_z_h as u16) << 8) | out_z_l as u16) & 0xFFFF;
160        let mag_z = self.adc_to_gauss(out_z_adc, SENSITIVITY_4);
161
162        Ok(mag_z)
163    }
164    pub fn read_mag(&self) -> Result<[f32; 3], LinuxI2CError> {
165        let mag_x = self.read_mag_x()?;
166        let mag_y = self.read_mag_y()?;
167        let mag_z = self.read_mag_z()?;
168        // let mag_t = (mag_x.powf(2.0) + mag_y.powf(2.0) + mag_z.powf(2.0)).sqrt();
169        let mag = [mag_x, mag_y, mag_z];
170
171        Ok(mag)
172    }
173    fn adc_to_gauss(&self, mut adc: u16, sensitifity: u16) -> f32 {
174        let mut sign: f32 = 1.0;
175        if adc >= 0x8000 {
176            sign = -1.0;
177            adc = 0xFFFF - adc;
178        }
179
180        adc as f32 * sign / sensitifity as f32
181    }
182    pub fn read_id(&self) -> Result<u8, LinuxI2CError> {
183        let mut dev = LinuxI2CDevice::new(&format!("/dev/i2c-{}", self.bus), self.address)?;
184        let id = dev.smbus_read_byte_data(WHO_AM_I as u8)?;
185
186        Ok(id)
187    }
188    pub fn read_temp(&self) -> Result<f32, LinuxI2CError> {
189        let mut dev = LinuxI2CDevice::new(&format!("/dev/i2c-{}", self.bus), self.address)?;
190        let temp_h = dev.smbus_read_byte_data(TEMP_OUT_H as u8)?;
191        let temp_l = dev.smbus_read_byte_data(TEMP_OUT_L as u8)?;
192        let mut temp_adc = (((temp_h as u16) << 8) | (temp_l as u16)) & 0xFFFF;
193
194        let mut sign: f32 = 1.0;
195        if temp_adc >= 0x8000 {
196            sign = -1.0;
197            temp_adc = 0xFFFF - temp_adc;
198        }
199
200        let temp = (temp_adc as f32) * sign / SENSITIVITY_TEMP as f32 + 25.0;
201
202        Ok(temp)
203    }
204}
tof_control/device/lis3mdltr.rs

tof_control/device/
lis3mdltr.rs
