omnitiles/drivers/
vl53l0x.rs

1// SPDX-License-Identifier: MIT
2// © 2025–2026 Christopher Liu
3
4//! VL53L0X Time-of-Flight sensor driver.
5
6use crate::hw::I2cBus;
7use cortex_m::asm;
8use stm32f7xx_hal::i2c::{self, PinScl, PinSda};
9use stm32f7xx_hal::pac::I2C1;
10
11const DEFAULT_ADDR: u8 = 0x29;
12
13#[derive(Debug)]
14pub enum Error {
15    I2c(i2c::Error),
16    InvalidDevice,
17    Timeout,
18}
19
20impl From<i2c::Error> for Error {
21    fn from(e: i2c::Error) -> Self {
22        Error::I2c(e)
23    }
24}
25
26pub struct Vl53l0x<SCL, SDA> {
27    bus: I2cBus<SCL, SDA>,
28    addr: u8,
29    stop_variable: u8,
30}
31
32impl<SCL, SDA> Vl53l0x<SCL, SDA>
33where
34    SCL: PinScl<I2C1>,
35    SDA: PinSda<I2C1>,
36{
37    /// Verify device identity and run mandatory data init (2V8 I/O, stop_variable, MSRC, sequence).
38    pub fn new(bus: I2cBus<SCL, SDA>) -> Result<Self, Error> {
39        let mut dev = Self {
40            bus,
41            addr: DEFAULT_ADDR,
42            stop_variable: 0,
43        };
44
45        // Soft reset; ignore errors (device may NACK if stuck on non-standard page).
46        let _ = dev.write_reg(0xFF, 0x00);
47        let _ = dev.write_reg(0x80, 0x00);
48        let _ = dev.write_reg(0xBF, 0x00);
49        asm::delay(1_000_000);
50        let _ = dev.write_reg(0xBF, 0x01);
51        asm::delay(1_000_000);
52
53        if dev.read_reg(0xC0)? != 0xEE {
54            return Err(Error::InvalidDevice);
55        }
56
57        // Set 2V8 I/O mode
58        let vhv = dev.read_reg(0x89)?;
59        dev.write_reg(0x89, vhv | 0x01)?;
60
61        // Mandatory private sequence — capture stop_variable
62        dev.write_reg(0x88, 0x00)?;
63        dev.write_reg(0x80, 0x01)?;
64        dev.write_reg(0xFF, 0x01)?;
65        dev.write_reg(0x00, 0x00)?;
66        dev.stop_variable = dev.read_reg(0x91)?;
67        dev.write_reg(0x00, 0x01)?;
68        dev.write_reg(0xFF, 0x00)?;
69        dev.write_reg(0x80, 0x00)?;
70
71        // Disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks
72        let msrc = dev.read_reg(0x60)?;
73        dev.write_reg(0x60, msrc | 0x12)?;
74
75        // Signal rate limit: 0.25 MCPS in 9.7 fixed-point format
76        dev.write_reg(0x44, 0x00)?;
77        dev.write_reg(0x45, 0x20)?;
78
79        dev.write_reg(0x01, 0xFF)?;
80
81        Ok(dev)
82    }
83
84    /// Read SPAD info from NVM and configure the SPAD enable map.
85    pub fn static_init(&mut self) -> Result<(), Error> {
86        // Enter NVM read mode via private registers
87        self.write_reg(0x80, 0x01)?;
88        self.write_reg(0xFF, 0x01)?;
89        self.write_reg(0x00, 0x00)?;
90        self.write_reg(0xFF, 0x06)?;
91        let r83 = self.read_reg(0x83)?;
92        self.write_reg(0x83, r83 | 0x04)?;
93        self.write_reg(0xFF, 0x07)?;
94        self.write_reg(0x81, 0x01)?;
95        self.write_reg(0x80, 0x01)?;
96        self.write_reg(0x94, 0x6B)?;
97        self.write_reg(0x83, 0x00)?;
98
99        // Wait for NVM read complete
100        let mut ready = false;
101        for _ in 0..50_000u32 {
102            if self.read_reg(0x83)? != 0x00 {
103                ready = true;
104                break;
105            }
106        }
107        if !ready {
108            return Err(Error::Timeout);
109        }
110
111        self.write_reg(0x83, 0x01)?;
112        let tmp = self.read_reg(0x92)?;
113        let spad_count = tmp & 0x7F;
114        let spad_is_aperture = (tmp >> 7) & 0x01 != 0;
115
116        // Exit NVM read mode
117        self.write_reg(0x81, 0x00)?;
118        self.write_reg(0xFF, 0x06)?;
119        let r83 = self.read_reg(0x83)?;
120        self.write_reg(0x83, r83 & !0x04)?;
121        self.write_reg(0xFF, 0x01)?;
122        self.write_reg(0x00, 0x01)?;
123        self.write_reg(0xFF, 0x00)?;
124        self.write_reg(0x80, 0x00)?;
125
126        self.write_reg(0xFF, 0x01)?;
127        self.write_reg(0x4F, 0x00)?;
128        self.write_reg(0x4E, 0x2C)?;
129        self.write_reg(0xFF, 0x00)?;
130        self.write_reg(0xB6, 0xB4)?;
131
132        // Read SPAD enable map (6 bytes from 0xB0)
133        let mut spad_map = [0u8; 6];
134        self.bus.write_read(self.addr, &[0xB0u8], &mut spad_map)?;
135
136        // Enable first spad_count SPADs (aperture from bit 12, else from 0)
137        let first_spad_to_enable: u8 = if spad_is_aperture { 12 } else { 0 };
138        let mut spads_enabled: u8 = 0;
139
140        for i in 0u8..48 {
141            let byte = (i / 8) as usize;
142            let bit = i % 8;
143            if i < first_spad_to_enable || spads_enabled >= spad_count {
144                spad_map[byte] &= !(1 << bit);
145            } else if spad_map[byte] & (1 << bit) != 0 {
146                spads_enabled += 1;
147            }
148        }
149
150        // Write back SPAD map (7-byte I2C transaction)
151        let mut buf = [0u8; 7];
152        buf[0] = 0xB0;
153        buf[1..7].copy_from_slice(&spad_map);
154        self.bus.write(self.addr, &buf)?;
155
156        Ok(())
157    }
158
159    /// Write ST tuning register block
160    pub fn load_tuning(&mut self) -> Result<(), Error> {
161        let settings: &[(u8, u8)] = &[
162            (0xFF, 0x01),
163            (0x00, 0x00),
164            (0xFF, 0x00),
165            (0x09, 0x00),
166            (0x10, 0x00),
167            (0x11, 0x00),
168            (0x24, 0x01),
169            (0x25, 0xFF),
170            (0x75, 0x00),
171            (0xFF, 0x01),
172            (0x4E, 0x2C),
173            (0x48, 0x00),
174            (0x30, 0x20),
175            (0xFF, 0x00),
176            (0x30, 0x09),
177            (0x54, 0x00),
178            (0x31, 0x04),
179            (0x32, 0x03),
180            (0x40, 0x83),
181            (0x46, 0x25),
182            (0x60, 0x00),
183            (0x27, 0x00),
184            (0x50, 0x06),
185            (0x51, 0x00),
186            (0x52, 0x96),
187            (0x56, 0x08),
188            (0x57, 0x30),
189            (0x61, 0x00),
190            (0x62, 0x00),
191            (0x64, 0x00),
192            (0x65, 0x00),
193            (0x66, 0xA0),
194            (0xFF, 0x01),
195            (0x22, 0x32),
196            (0x47, 0x14),
197            (0x49, 0xFF),
198            (0x4A, 0x00),
199            (0xFF, 0x00),
200            (0x7A, 0x0A),
201            (0x7B, 0x00),
202            (0x78, 0x21),
203            (0xFF, 0x01),
204            (0x23, 0x34),
205            (0x42, 0x00),
206            (0x44, 0xFF),
207            (0x45, 0x26),
208            (0x46, 0x05),
209            (0x40, 0x40),
210            (0x0E, 0x06),
211            (0x20, 0x1A),
212            (0x43, 0x40),
213            (0xFF, 0x00),
214            (0x34, 0x03),
215            (0x35, 0x44),
216            (0xFF, 0x01),
217            (0x31, 0x04),
218            (0x4B, 0x09),
219            (0x4C, 0x05),
220            (0x4D, 0x04),
221            (0xFF, 0x00),
222            (0x44, 0x00),
223            (0x45, 0x20),
224            (0x47, 0x08),
225            (0x48, 0x28),
226            (0x67, 0x00),
227            (0x70, 0x04),
228            (0x71, 0x01),
229            (0x72, 0xFE),
230            (0x76, 0x00),
231            (0x77, 0x00),
232            (0xFF, 0x01),
233            (0x0D, 0x01),
234            (0xFF, 0x00),
235            (0x80, 0x01),
236            (0x01, 0xF8),
237            (0xFF, 0x01),
238            (0x8E, 0x01),
239            (0x00, 0x01),
240            (0xFF, 0x00),
241            (0x80, 0x00),
242        ];
243        for &(reg, val) in settings {
244            self.write_reg(reg, val)?;
245        }
246        Ok(())
247    }
248
249    /// VHV and phase calibration
250    pub fn calibrate(&mut self) -> Result<(), Error> {
251        self.write_reg(0x0A, 0x04)?;
252        let gpio_hv = self.read_reg(0x84)?;
253        self.write_reg(0x84, gpio_hv & !0x10)?;
254        self.write_reg(0x0B, 0x01)?;
255
256        self.write_reg(0x01, 0xFF)?;
257
258        self.write_reg(0x01, 0x01)?;
259        self.perform_single_ref_calibration(0x40)?;
260
261        self.write_reg(0x01, 0x02)?;
262        self.perform_single_ref_calibration(0x00)?;
263
264        self.write_reg(0x01, 0xFF)?;
265
266        Ok(())
267    }
268
269    /// Single-shot range measurement in mm
270    pub fn read_range_mm(&mut self) -> Result<u16, Error> {
271        // Single-shot start sequence
272        self.write_reg(0x80, 0x01)?;
273        self.write_reg(0xFF, 0x01)?;
274        self.write_reg(0x00, 0x00)?;
275        self.write_reg(0x91, self.stop_variable)?;
276        self.write_reg(0x00, 0x01)?;
277        self.write_reg(0xFF, 0x00)?;
278        self.write_reg(0x80, 0x00)?;
279
280        self.write_reg(0x00, 0x01)?;
281
282        for _ in 0..50_000u32 {
283            if self.read_reg(0x00)? & 0x01 == 0 {
284                break;
285            }
286        }
287
288        let mut done = false;
289        for _ in 0..50_000u32 {
290            if self.read_reg(0x13)? & 0x07 != 0 {
291                done = true;
292                break;
293            }
294        }
295        if !done {
296            return Err(Error::Timeout);
297        }
298
299        let mut buf = [0u8; 2];
300        self.bus.write_read(self.addr, &[0x1Eu8], &mut buf)?;
301        let mm = ((buf[0] as u16) << 8) | buf[1] as u16;
302
303        self.write_reg(0x0B, 0x01)?;
304
305        Ok(mm)
306    }
307
308    fn perform_single_ref_calibration(&mut self, vhv_init_byte: u8) -> Result<(), Error> {
309        self.write_reg(0x00, 0x01 | vhv_init_byte)?;
310
311        let mut done = false;
312        for _ in 0..50_000u32 {
313            if self.read_reg(0x13)? & 0x07 != 0 {
314                done = true;
315                break;
316            }
317        }
318        if !done {
319            return Err(Error::Timeout);
320        }
321
322        self.write_reg(0x0B, 0x01)?;
323        self.write_reg(0x00, 0x00)?;
324
325        Ok(())
326    }
327
328    fn write_reg(&mut self, reg: u8, val: u8) -> Result<(), Error> {
329        self.bus.write(self.addr, &[reg, val]).map_err(Error::I2c)
330    }
331
332    fn read_reg(&mut self, reg: u8) -> Result<u8, Error> {
333        let mut buf = [0u8; 1];
334        self.bus.write_read(self.addr, &[reg], &mut buf)?;
335        Ok(buf[0])
336    }
337}
omnitiles/drivers/vl53l0x.rs

omnitiles/drivers/
vl53l0x.rs
