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        // Probe first — if no device is present this returns an I2C NACK immediately
46        // instead of hanging during the soft reset sequence below.
47        if dev.read_reg(0xC0)? != 0xEE {
48            return Err(Error::InvalidDevice);
49        }
50
51        // Soft reset via SOFT_RESET_GO2_SOFT_RESET_N (0xBF): pull low then high.
52        // Page-select (0xFF) and power (0x80) writes ensure the device is in a known
53        // state. Errors are ignored because the device may NACK during reset.
54        let _ = dev.write_reg(0xFF, 0x00);
55        let _ = dev.write_reg(0x80, 0x00);
56        let _ = dev.write_reg(0xBF, 0x00);
57        asm::delay(1_000_000);
58        let _ = dev.write_reg(0xBF, 0x01);
59        asm::delay(1_000_000);
60
61        // Re-verify after reset
62        if dev.read_reg(0xC0)? != 0xEE {
63            return Err(Error::InvalidDevice);
64        }
65
66        // VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV — set bit 0 to enable 2.8V I/O mode
67        let vhv = dev.read_reg(0x89)?;
68        dev.write_reg(0x89, vhv | 0x01)?;
69
70        // Capture stop_variable from undocumented register 0x91.
71        // This device-specific value is needed later to start single-shot measurements.
72        dev.write_reg(0x88, 0x00)?;
73        dev.write_reg(0x80, 0x01)?;
74        dev.write_reg(0xFF, 0x01)?;
75        dev.write_reg(0x00, 0x00)?;
76        dev.stop_variable = dev.read_reg(0x91)?;
77        dev.write_reg(0x00, 0x01)?;
78        dev.write_reg(0xFF, 0x00)?;
79        dev.write_reg(0x80, 0x00)?;
80
81        // MSRC_CONFIG_CONTROL — disable SIGNAL_RATE_MSRC (bit 1) and
82        // SIGNAL_RATE_PRE_RANGE (bit 4) limit checks to avoid rejecting valid readings
83        let msrc = dev.read_reg(0x60)?;
84        dev.write_reg(0x60, msrc | 0x12)?;
85
86        // FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT — 0.25 MCPS (9.7 fixed-point)
87        dev.write_reg(0x44, 0x00)?;
88        dev.write_reg(0x45, 0x20)?;
89
90        dev.write_reg(0x01, 0xFF)?;
91
92        Ok(dev)
93    }
94
95    /// Read SPAD info from NVM and configure the SPAD enable map.
96    pub fn static_init(&mut self) -> Result<(), Error> {
97        // Enter NVM read mode via private registers
98        self.write_reg(0x80, 0x01)?;
99        self.write_reg(0xFF, 0x01)?;
100        self.write_reg(0x00, 0x00)?;
101        self.write_reg(0xFF, 0x06)?;
102        let r83 = self.read_reg(0x83)?;
103        self.write_reg(0x83, r83 | 0x04)?;
104        self.write_reg(0xFF, 0x07)?;
105        self.write_reg(0x81, 0x01)?;
106        self.write_reg(0x80, 0x01)?;
107        self.write_reg(0x94, 0x6B)?;
108        self.write_reg(0x83, 0x00)?;
109
110        // Wait for NVM read complete
111        let mut ready = false;
112        for _ in 0..50_000u32 {
113            if self.read_reg(0x83)? != 0x00 {
114                ready = true;
115                break;
116            }
117        }
118        if !ready {
119            return Err(Error::Timeout);
120        }
121
122        self.write_reg(0x83, 0x01)?;
123        let tmp = self.read_reg(0x92)?;
124        let spad_count = tmp & 0x7F;
125        let spad_is_aperture = (tmp >> 7) & 0x01 != 0;
126
127        // Exit NVM read mode
128        self.write_reg(0x81, 0x00)?;
129        self.write_reg(0xFF, 0x06)?;
130        let r83 = self.read_reg(0x83)?;
131        self.write_reg(0x83, r83 & !0x04)?;
132        self.write_reg(0xFF, 0x01)?;
133        self.write_reg(0x00, 0x01)?;
134        self.write_reg(0xFF, 0x00)?;
135        self.write_reg(0x80, 0x00)?;
136
137        self.write_reg(0xFF, 0x01)?;
138        self.write_reg(0x4F, 0x00)?;
139        self.write_reg(0x4E, 0x2C)?;
140        self.write_reg(0xFF, 0x00)?;
141        self.write_reg(0xB6, 0xB4)?;
142
143        // Read SPAD enable map (6 bytes from 0xB0)
144        let mut spad_map = [0u8; 6];
145        self.bus.write_read(self.addr, &[0xB0u8], &mut spad_map)?;
146
147        // Enable first spad_count SPADs (aperture from bit 12, else from 0)
148        let first_spad_to_enable: u8 = if spad_is_aperture { 12 } else { 0 };
149        let mut spads_enabled: u8 = 0;
150
151        for i in 0u8..48 {
152            let byte = (i / 8) as usize;
153            let bit = i % 8;
154            if i < first_spad_to_enable || spads_enabled >= spad_count {
155                spad_map[byte] &= !(1 << bit);
156            } else if spad_map[byte] & (1 << bit) != 0 {
157                spads_enabled += 1;
158            }
159        }
160
161        // Write back SPAD map (7-byte I2C transaction)
162        let mut buf = [0u8; 7];
163        buf[0] = 0xB0;
164        buf[1..7].copy_from_slice(&spad_map);
165        self.bus.write(self.addr, &buf)?;
166
167        Ok(())
168    }
169
170    /// Write ST's default tuning register table.
171    ///
172    /// This is a magic blob copied verbatim from ST's proprietary VL53L0X API
173    /// (vl53l0x_tuning.h). It configures internal analog/digital parameters that
174    /// ST does not publicly document. Every open-source VL53L0X driver reproduces
175    /// this table as-is. Do not modify individual values.
176    pub fn load_tuning(&mut self) -> Result<(), Error> {
177        let settings: &[(u8, u8)] = &[
178            (0xFF, 0x01),
179            (0x00, 0x00),
180            (0xFF, 0x00),
181            (0x09, 0x00),
182            (0x10, 0x00),
183            (0x11, 0x00),
184            (0x24, 0x01),
185            (0x25, 0xFF),
186            (0x75, 0x00),
187            (0xFF, 0x01),
188            (0x4E, 0x2C),
189            (0x48, 0x00),
190            (0x30, 0x20),
191            (0xFF, 0x00),
192            (0x30, 0x09),
193            (0x54, 0x00),
194            (0x31, 0x04),
195            (0x32, 0x03),
196            (0x40, 0x83),
197            (0x46, 0x25),
198            (0x60, 0x00),
199            (0x27, 0x00),
200            (0x50, 0x06),
201            (0x51, 0x00),
202            (0x52, 0x96),
203            (0x56, 0x08),
204            (0x57, 0x30),
205            (0x61, 0x00),
206            (0x62, 0x00),
207            (0x64, 0x00),
208            (0x65, 0x00),
209            (0x66, 0xA0),
210            (0xFF, 0x01),
211            (0x22, 0x32),
212            (0x47, 0x14),
213            (0x49, 0xFF),
214            (0x4A, 0x00),
215            (0xFF, 0x00),
216            (0x7A, 0x0A),
217            (0x7B, 0x00),
218            (0x78, 0x21),
219            (0xFF, 0x01),
220            (0x23, 0x34),
221            (0x42, 0x00),
222            (0x44, 0xFF),
223            (0x45, 0x26),
224            (0x46, 0x05),
225            (0x40, 0x40),
226            (0x0E, 0x06),
227            (0x20, 0x1A),
228            (0x43, 0x40),
229            (0xFF, 0x00),
230            (0x34, 0x03),
231            (0x35, 0x44),
232            (0xFF, 0x01),
233            (0x31, 0x04),
234            (0x4B, 0x09),
235            (0x4C, 0x05),
236            (0x4D, 0x04),
237            (0xFF, 0x00),
238            (0x44, 0x00),
239            (0x45, 0x20),
240            (0x47, 0x08),
241            (0x48, 0x28),
242            (0x67, 0x00),
243            (0x70, 0x04),
244            (0x71, 0x01),
245            (0x72, 0xFE),
246            (0x76, 0x00),
247            (0x77, 0x00),
248            (0xFF, 0x01),
249            (0x0D, 0x01),
250            (0xFF, 0x00),
251            (0x80, 0x01),
252            (0x01, 0xF8),
253            (0xFF, 0x01),
254            (0x8E, 0x01),
255            (0x00, 0x01),
256            (0xFF, 0x00),
257            (0x80, 0x00),
258        ];
259        for &(reg, val) in settings {
260            self.write_reg(reg, val)?;
261        }
262        Ok(())
263    }
264
265    /// Run VHV (voltage) and phase reference calibration.
266    ///
267    /// Must be called after `load_tuning`. The two calibration passes (VHV = 0x40,
268    /// phase = 0x00) compensate for chip-to-chip variation. Register 0x01 is
269    /// SYSTEM_SEQUENCE_CONFIG — each write selects which calibration step runs.
270    pub fn calibrate(&mut self) -> Result<(), Error> {
271        // SYSRANGE_START config and clear GPIO interrupt
272        self.write_reg(0x0A, 0x04)?;
273        let gpio_hv = self.read_reg(0x84)?;
274        self.write_reg(0x84, gpio_hv & !0x10)?;
275        self.write_reg(0x0B, 0x01)?;
276
277        self.write_reg(0x01, 0xFF)?;
278
279        // VHV calibration
280        self.write_reg(0x01, 0x01)?;
281        self.perform_single_ref_calibration(0x40)?;
282
283        // Phase calibration
284        self.write_reg(0x01, 0x02)?;
285        self.perform_single_ref_calibration(0x00)?;
286
287        // Restore full sequence config
288        self.write_reg(0x01, 0xFF)?;
289
290        Ok(())
291    }
292
293    /// Single-shot range measurement in mm
294    pub fn read_range_mm(&mut self) -> Result<u16, Error> {
295        // Write stop_variable (captured during init) then trigger single-shot via
296        // SYSRANGE_START (0x00)
297        self.write_reg(0x80, 0x01)?;
298        self.write_reg(0xFF, 0x01)?;
299        self.write_reg(0x00, 0x00)?;
300        self.write_reg(0x91, self.stop_variable)?;
301        self.write_reg(0x00, 0x01)?;
302        self.write_reg(0xFF, 0x00)?;
303        self.write_reg(0x80, 0x00)?;
304
305        self.write_reg(0x00, 0x01)?;
306
307        // Wait for SYSRANGE_START bit 0 to clear (device accepted the command)
308        let mut started = false;
309        for _ in 0..50_000u32 {
310            if self.read_reg(0x00)? & 0x01 == 0 {
311                started = true;
312                break;
313            }
314        }
315        if !started {
316            return Err(Error::Timeout);
317        }
318
319        // Wait for RESULT_INTERRUPT_STATUS (0x13) — bits [2:0] != 0 means data ready
320        let mut done = false;
321        for _ in 0..50_000u32 {
322            if self.read_reg(0x13)? & 0x07 != 0 {
323                done = true;
324                break;
325            }
326        }
327        if !done {
328            return Err(Error::Timeout);
329        }
330
331        // Read 16-bit range from RESULT_RANGE_STATUS + 10 (0x1E..0x1F)
332        let mut buf = [0u8; 2];
333        self.bus.write_read(self.addr, &[0x1Eu8], &mut buf)?;
334        let mm = ((buf[0] as u16) << 8) | buf[1] as u16;
335
336        // SYSTEM_INTERRUPT_CLEAR — acknowledge the interrupt
337        self.write_reg(0x0B, 0x01)?;
338
339        Ok(mm)
340    }
341
342    /// Start a calibration via SYSRANGE_START, poll RESULT_INTERRUPT_STATUS until
343    /// complete, then clear the interrupt. `vhv_init_byte` selects the cal type:
344    /// 0x40 = VHV, 0x00 = phase.
345    fn perform_single_ref_calibration(&mut self, vhv_init_byte: u8) -> Result<(), Error> {
346        self.write_reg(0x00, 0x01 | vhv_init_byte)?;
347
348        let mut done = false;
349        for _ in 0..50_000u32 {
350            if self.read_reg(0x13)? & 0x07 != 0 {
351                done = true;
352                break;
353            }
354        }
355        if !done {
356            return Err(Error::Timeout);
357        }
358
359        self.write_reg(0x0B, 0x01)?;
360        self.write_reg(0x00, 0x00)?;
361
362        Ok(())
363    }
364
365    fn write_reg(&mut self, reg: u8, val: u8) -> Result<(), Error> {
366        self.bus.write(self.addr, &[reg, val]).map_err(Error::I2c)
367    }
368
369    fn read_reg(&mut self, reg: u8) -> Result<u8, Error> {
370        let mut buf = [0u8; 1];
371        self.bus.write_read(self.addr, &[reg], &mut buf)?;
372        Ok(buf[0])
373    }
374}
omnitiles/drivers/vl53l0x.rs

omnitiles/drivers/
vl53l0x.rs
