vl53l0x/
lib.rs

1//! vl53l0x --https://www.st.com/resource/en/datasheet/vl53l0x.pdf -- Time of Flight IR distance sensor.
2
3#![deny(missing_docs)]
4#![deny(warnings)]
5#![allow(dead_code)]
6#![no_std]
7
8use hal::i2c::I2c;
9
10const DEFAULT_ADDRESS: u8 = 0x29;
11
12/// dummy
13pub struct VL53L0x<I2C: I2c> {
14    com: I2C,
15    /// dummy
16    pub revision_id: u8,
17    io_mode2v8: bool,
18    stop_variable: u8,
19    measurement_timing_budget_microseconds: u32,
20    address: u8,
21}
22
23/// MPU Error
24#[derive(Debug, Copy, Clone)]
25pub enum Error<E> {
26    /// WHO_AM_I returned invalid value (returned value is argument).
27    InvalidDevice(u8),
28    /// Underlying bus error.
29    BusError(E),
30    /// Timeout
31    Timeout,
32    /// I2C address not valid, needs to be between 0x08 and 0x77.
33    /// It is a 7 bit address thus the range is 0x00 - 0x7F but
34    /// 0x00 - 0x07 and 0x78 - 0x7F are reserved I2C addresses and cannot be used.
35    InvalidAddress(u8),
36}
37
38impl<E> core::convert::From<E> for Error<E> {
39    fn from(error: E) -> Self {
40        Error::BusError(error)
41    }
42}
43
44impl<I2C, E> VL53L0x<I2C>
45where
46    I2C: I2c<Error = E>,
47{
48    /// Creates new driver with default address.
49    pub fn new(i2c: I2C) -> Result<VL53L0x<I2C>, Error<E>>
50    where
51        I2C: I2c<Error = E>,
52    {
53        VL53L0x::with_address(i2c, DEFAULT_ADDRESS)
54    }
55
56    /// Creates new driver with given address.
57    pub fn with_address(i2c: I2C, address: u8) -> Result<VL53L0x<I2C>, Error<E>>
58    where
59        I2C: I2c<Error = E>,
60    {
61        let mut chip = VL53L0x {
62            com: i2c,
63            revision_id: 0x00,
64            io_mode2v8: true,
65            stop_variable: 0,
66            measurement_timing_budget_microseconds: 0,
67            address,
68        };
69
70        let wai = chip.who_am_i()?;
71        if wai == 0xEE {
72            chip.init_hardware()?;
73            // FIXME: return an error/optional
74            /*
75            chip.set_high_i2c_voltage(); // TODO: make configurable
76            chip.revision_id = chip.read_revision_id();
77            chip.reset();
78            chip.set_high_i2c_voltage();
79            chip.set_standard_i2c_mode(); // TODO: make configurable
80             */
81            Ok(chip)
82        } else {
83            Err(Error::InvalidDevice(wai))
84        }
85    }
86
87    /// Changes the I2C address of the sensor.
88    /// Note that the address resets when the device is powered off.
89    /// Only allows values between 0x08 and 0x77 as the device uses a 7 bit address and
90    /// 0x00 - 0x07 and 0x78 - 0x7F are reserved
91    pub fn set_address(&mut self, new_address: u8) -> Result<(), Error<E>> {
92        if new_address < 0x08 || new_address > 0x77 {
93            return Err(Error::InvalidAddress(new_address));
94        }
95        self.com.write(
96            self.address,
97            &[Register::I2C_SLAVE_DEVICE_ADDRESS as u8, new_address],
98        )?;
99        self.address = new_address;
100
101        Ok(())
102    }
103
104    fn power_on(&mut self) -> Result<(), E> {
105        // TODO use pin to poweron
106        // XXX: not-needed?
107        Ok(())
108    }
109
110    fn read_register(&mut self, reg: Register) -> Result<u8, E> {
111        let mut data: [u8; 1] = [0];
112        // FIXME:
113        //  * device address is not a const
114        //  * register address is u16
115        self.com.write_read(self.address, &[reg as u8], &mut data)?;
116        Ok(data[0])
117    }
118
119    fn read_byte(&mut self, reg: u8) -> Result<u8, E> {
120        let mut data: [u8; 1] = [0];
121        // FIXME:
122        //  * device address is not a const
123        //  * register address is u16
124        self.com.write_read(self.address, &[reg], &mut data)?;
125        Ok(data[0])
126    }
127
128    fn read_6bytes(&mut self, reg: Register) -> Result<[u8; 6], E> {
129        let mut ret: [u8; 6] = Default::default();
130        self.read_registers(reg, &mut ret)?;
131
132        Ok(ret)
133    }
134
135    fn read_registers(
136        &mut self,
137        reg: Register,
138        buffer: &mut [u8],
139    ) -> Result<(), E> {
140        // const I2C_AUTO_INCREMENT: u8 = 1 << 7;
141        const I2C_AUTO_INCREMENT: u8 = 0;
142        self.com.write_read(
143            self.address,
144            &[(reg as u8) | I2C_AUTO_INCREMENT],
145            buffer,
146        )?;
147
148        Ok(())
149    }
150
151    fn read_16bit(&mut self, reg: Register) -> Result<u16, E> {
152        let mut buffer: [u8; 2] = [0, 0];
153        self.read_registers(reg, &mut buffer)?;
154        Ok(((buffer[0] as u16) << 8) + (buffer[1] as u16))
155    }
156
157    fn write_byte(&mut self, reg: u8, byte: u8) -> Result<(), E> {
158        let mut buffer = [0];
159        self.com.write_read(self.address, &[reg, byte], &mut buffer)
160    }
161
162    fn write_register(&mut self, reg: Register, byte: u8) -> Result<(), E> {
163        let mut buffer = [0];
164        self.com
165            .write_read(self.address, &[reg as u8, byte], &mut buffer)
166    }
167
168    fn write_6bytes(&mut self, reg: Register, bytes: [u8; 6]) -> Result<(), E> {
169        let mut buf: [u8; 6] = [0, 0, 0, 0, 0, 0];
170        self.com.write_read(
171            self.address,
172            &[
173                reg as u8, bytes[0], bytes[1], bytes[2], bytes[3], bytes[4],
174                bytes[5],
175            ],
176            &mut buf,
177        )
178    }
179
180    fn write_16bit(&mut self, reg: Register, word: u16) -> Result<(), E> {
181        let mut buffer = [0];
182        let msb = (word >> 8) as u8;
183        let lsb = (word & 0xFF) as u8;
184        self.com
185            .write_read(self.address, &[reg as u8, msb, lsb], &mut buffer)
186    }
187
188    fn write_32bit(&mut self, reg: Register, word: u32) -> Result<(), E> {
189        let mut buffer = [0];
190        let v1 = (word & 0xFF) as u8;
191        let v2 = ((word >> 8) & 0xFF) as u8;
192        let v3 = ((word >> 16) & 0xFF) as u8;
193        let v4 = ((word >> 24) & 0xFF) as u8;
194        self.com.write_read(
195            self.address,
196            &[reg as u8, v1, v2, v3, v4],
197            &mut buffer,
198        )
199    }
200
201    fn set_signal_rate_limit(&mut self, limit: f32) -> Result<bool, E> {
202        if limit < 0.0 || limit > 511.99 {
203            Ok(false)
204        } else {
205            // Q9.7 fixed point format (9 integer bits, 7 fractional bits)
206            self.write_16bit(
207                Register::FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
208                (limit * ((1 << 7) as f32)) as u16,
209            )?;
210            Ok(true)
211        }
212    }
213
214    fn get_spad_info(&mut self) -> Result<(u8, u8), Error<E>> {
215        self.write_byte(0x80, 0x01)?;
216        self.write_byte(0xFF, 0x01)?;
217        self.write_byte(0x00, 0x00)?;
218
219        self.write_byte(0xFF, 0x06)?;
220        let mut tmp83 = self.read_byte(0x83)?;
221        self.write_byte(0x83, tmp83 | 0x04)?;
222        self.write_byte(0xFF, 0x07)?;
223        self.write_byte(0x81, 0x01)?;
224
225        self.write_byte(0x80, 0x01)?;
226
227        self.write_byte(0x94, 0x6b)?;
228        self.write_byte(0x83, 0x00)?;
229
230        let mut c = 0;
231        while self.read_byte(0x83)? == 0x00 {
232            c += 1;
233            if c == 65535 {
234                return Err(Error::Timeout);
235            }
236        }
237
238        self.write_byte(0x83, 0x01)?;
239        let tmp = self.read_byte(0x92)?;
240
241        let count: u8 = tmp & 0x7f;
242        let type_is_aperture: u8 = (tmp >> 7) & 0x01;
243
244        self.write_byte(0x81, 0x00)?;
245        self.write_byte(0xFF, 0x06)?;
246        tmp83 = self.read_byte(0x83)?;
247        self.write_byte(0x83, tmp83 & !0x04)?;
248        self.write_byte(0xFF, 0x01)?;
249        self.write_byte(0x00, 0x01)?;
250
251        self.write_byte(0xFF, 0x00)?;
252        self.write_byte(0x80, 0x00)?;
253
254        Ok((count, type_is_aperture))
255    }
256
257    /// startContinuous
258    pub fn start_continuous(&mut self, period_millis: u32) -> Result<(), E> {
259        self.write_byte(0x80, 0x01)?;
260        self.write_byte(0xFF, 0x01)?;
261        self.write_byte(0x00, 0x00)?;
262        let sv = self.stop_variable;
263        self.write_byte(0x91, sv)?;
264        self.write_byte(0x00, 0x01)?;
265        self.write_byte(0xFF, 0x00)?;
266        self.write_byte(0x80, 0x00)?;
267
268        let mut period_millis = period_millis;
269        if period_millis != 0 {
270            // continuous timed mode
271            // VL53L0X_SetInterMeasurementPeriodMilliSeconds() begin
272            let osc_calibrate_value =
273                self.read_16bit(Register::OSC_CALIBRATE_VAL)?;
274
275            if osc_calibrate_value != 0 {
276                period_millis *= osc_calibrate_value as u32;
277            }
278
279            self.write_32bit(
280                Register::SYSTEM_INTERMEASUREMENT_PERIOD,
281                period_millis,
282            )?;
283            // VL53L0X_SetInterMeasurementPeriodMilliSeconds() end
284            // VL53L0X_REG_SYSRANGE_MODE_TIMED
285            self.write_register(Register::SYSRANGE_START, 0x04)?;
286        } else {
287            // continuous back-to-back mode
288            // VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK
289            self.write_register(Register::SYSRANGE_START, 0x02)?;
290        }
291
292        Ok(())
293    }
294
295    /// stopContinuous()
296    pub fn stop_continuous(&mut self) -> Result<(), E> {
297        // VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT
298        self.write_register(Register::SYSRANGE_START, 0x01)?;
299        self.write_byte(0xFF, 0x01)?;
300        self.write_byte(0x00, 0x00)?;
301        self.write_byte(0x91, 0x00)?;
302        self.write_byte(0x00, 0x01)?;
303        self.write_byte(0xFF, 0x00)?;
304
305        Ok(())
306    }
307
308    /// reads and returns range measurement or nb::Error::WouldBlock if it's not ready yet
309    pub fn read_range_mm(&mut self) -> nb::Result<u16, Error<E>> {
310        match self.read_register(Register::RESULT_INTERRUPT_STATUS) {
311            Ok(r) => {
312                if (r & 0x07) == 0 {
313                    Err(nb::Error::WouldBlock)
314                } else {
315                    let range_err =
316                        self.read_16bit(Register::RESULT_RANGE_STATUS_plus_10);
317                    let write_err = self
318                        .write_register(Register::SYSTEM_INTERRUPT_CLEAR, 0x01);
319                    match (range_err, write_err) {
320                        (Ok(res), Ok(_)) => Ok(res),
321                        (Err(e), _) => Err(nb::Error::Other(Error::from(e))),
322                        (_, Err(e)) => Err(nb::Error::Other(Error::from(e))),
323                    }
324                }
325            }
326            Err(e) => Err(nb::Error::Other(Error::from(e))),
327        }
328    }
329
330    /// readRangeContinuousMillimeters (blocking)
331    pub fn read_range_continuous_millimeters_blocking(
332        &mut self,
333    ) -> Result<u16, Error<E>> {
334        let mut c = 0;
335        while (self.read_register(Register::RESULT_INTERRUPT_STATUS)? & 0x07)
336            == 0
337        {
338            c += 1;
339            if c == 10000 {
340                return Err(Error::Timeout);
341            }
342        }
343        let range_err = self.read_16bit(Register::RESULT_RANGE_STATUS_plus_10);
344        // don't use ? to cleanup
345        self.write_register(Register::SYSTEM_INTERRUPT_CLEAR, 0x01)?;
346
347        Ok(range_err?)
348    }
349
350    /// readRangeSingleMillimeters (blocking)
351    pub fn read_range_single_millimeters_blocking(
352        &mut self,
353    ) -> Result<u16, Error<E>> {
354        self.write_byte(0x80, 0x01)?;
355        self.write_byte(0xFF, 0x01)?;
356        self.write_byte(0x00, 0x00)?;
357        let sv = self.stop_variable;
358        self.write_byte(0x91, sv)?;
359        self.write_byte(0x00, 0x01)?;
360        self.write_byte(0xFF, 0x00)?;
361        self.write_byte(0x80, 0x00)?;
362
363        self.write_register(Register::SYSRANGE_START, 0x01)?;
364
365        // "Wait until start bit has been cleared"
366        let mut c = 0;
367        while (self.read_register(Register::SYSRANGE_START)? & 0x01) != 0 {
368            c += 1;
369            if c == 10000 {
370                return Err(Error::Timeout);
371            }
372        }
373        self.read_range_continuous_millimeters_blocking()
374    }
375
376    // performSingleRefCalibration(uint8_t vhvInitByte)
377    fn perform_single_ref_calibration(
378        &mut self,
379        vhv_init_byte: u8,
380    ) -> Result<(), Error<E>> {
381        // VL53L0X_REG_SYSRANGE_MODE_START_STOP
382        self.write_register(Register::SYSRANGE_START, 0x01 | vhv_init_byte)?;
383        let mut c = 0;
384        while (self.read_register(Register::RESULT_INTERRUPT_STATUS)? & 0x07)
385            == 0
386        {
387            c += 1;
388            if c == 10000 {
389                return Err(Error::Timeout);
390            }
391        }
392        self.write_register(Register::SYSTEM_INTERRUPT_CLEAR, 0x01)?;
393        self.write_register(Register::SYSRANGE_START, 0x00)?;
394
395        Ok(())
396    }
397
398    fn init_hardware(&mut self) -> Result<(), Error<E>> {
399        // Enable the sensor
400
401        self.power_on()?;
402        // VL53L0X_DataInit() begin
403
404        // Sensor uses 1V8 mode for I/O by default; switch to 2V8 mode if necessary
405        if self.io_mode2v8 {
406            // set bit 0
407            let ext_sup_hv = self
408                .read_register(Register::VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV)?;
409            self.write_register(
410                Register::VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV,
411                ext_sup_hv | 0x01,
412            )?;
413        }
414
415        // "Set I2C standard mode"
416        self.write_byte(0x88, 0x00)?;
417        self.write_byte(0x80, 0x01)?;
418        self.write_byte(0xFF, 0x01)?;
419        self.write_byte(0x00, 0x00)?;
420        self.stop_variable = self.read_byte(0x91)?;
421        self.write_byte(0x00, 0x01)?;
422        self.write_byte(0xFF, 0x00)?;
423        self.write_byte(0x80, 0x00)?;
424
425        // disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks
426        let config = self.read_register(Register::MSRC_CONFIG_CONTROL)?;
427        self.write_register(Register::MSRC_CONFIG_CONTROL, config | 0x12)?;
428
429        // set final range signal rate limit to 0.25 MCPS (million counts per second)
430        self.set_signal_rate_limit(0.25)?;
431
432        self.write_register(Register::SYSTEM_SEQUENCE_CONFIG, 0xFF)?;
433
434        // VL53L0X_DataInit() end
435
436        // VL53L0X_StaticInit() begin
437
438        // TODO fail to initialize on timeout of this
439        let (spad_count, spad_type_is_aperture) = self.get_spad_info()?;
440
441        // The SPAD map (RefGoodSpadMap) is read by VL53L0X_get_info_from_device() in the API,
442        // but the same data seems to be more easily readable from GLOBAL_CONFIG_SPAD_ENABLES_REF_0 through _6, so read it from there
443        let mut ref_spad_map =
444            self.read_6bytes(Register::GLOBAL_CONFIG_SPAD_ENABLES_REF_0)?;
445
446        // -- VL53L0X_set_reference_spads() begin (assume NVM values are valid)
447
448        self.write_byte(0xFF, 0x01)?;
449        self.write_register(Register::DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00)?;
450        self.write_register(
451            Register::DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD,
452            0x2C,
453        )?;
454        self.write_byte(0xFF, 0x00)?;
455        self.write_register(Register::GLOBAL_CONFIG_REF_EN_START_SELECT, 0xB4)?;
456
457        // 12 is the first aperture spad
458        let first_spad_to_enable =
459            if spad_type_is_aperture != 0 { 12 } else { 0 };
460        let mut spads_enabled: u8 = 0;
461
462        for i in 0..48 {
463            if i < first_spad_to_enable || spads_enabled == spad_count {
464                // This bit is lower than the first one that should be enabled, or (reference_spad_count) bits have already been enabled, so zero this bit
465                ref_spad_map[i / 8] &= !(1 << (i % 8));
466            } else if (ref_spad_map[i / 8] >> (i % 8)) & 0x1 > 0 {
467                spads_enabled = spads_enabled + 1;
468            }
469        }
470
471        self.write_6bytes(
472            Register::GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
473            ref_spad_map,
474        )?;
475
476        // -- VL53L0X_set_reference_spads() end
477
478        // -- VL53L0X_load_tuning_settings() begin
479        // DefaultTuningSettings from vl53l0x_tuning.h
480
481        self.write_byte(0xFF, 0x01)?;
482        self.write_byte(0x00, 0x00)?;
483
484        self.write_byte(0xFF, 0x00)?;
485        self.write_byte(0x09, 0x00)?;
486        self.write_byte(0x10, 0x00)?;
487        self.write_byte(0x11, 0x00)?;
488
489        self.write_byte(0x24, 0x01)?;
490        self.write_byte(0x25, 0xFF)?;
491        self.write_byte(0x75, 0x00)?;
492
493        self.write_byte(0xFF, 0x01)?;
494        self.write_byte(0x4E, 0x2C)?;
495        self.write_byte(0x48, 0x00)?;
496        self.write_byte(0x30, 0x20)?;
497
498        self.write_byte(0xFF, 0x00)?;
499        self.write_byte(0x30, 0x09)?;
500        self.write_byte(0x54, 0x00)?;
501        self.write_byte(0x31, 0x04)?;
502        self.write_byte(0x32, 0x03)?;
503        self.write_byte(0x40, 0x83)?;
504        self.write_byte(0x46, 0x25)?;
505        self.write_byte(0x60, 0x00)?;
506        self.write_byte(0x27, 0x00)?;
507        self.write_byte(0x50, 0x06)?;
508        self.write_byte(0x51, 0x00)?;
509        self.write_byte(0x52, 0x96)?;
510        self.write_byte(0x56, 0x08)?;
511        self.write_byte(0x57, 0x30)?;
512        self.write_byte(0x61, 0x00)?;
513        self.write_byte(0x62, 0x00)?;
514        self.write_byte(0x64, 0x00)?;
515        self.write_byte(0x65, 0x00)?;
516        self.write_byte(0x66, 0xA0)?;
517
518        self.write_byte(0xFF, 0x01)?;
519        self.write_byte(0x22, 0x32)?;
520        self.write_byte(0x47, 0x14)?;
521        self.write_byte(0x49, 0xFF)?;
522        self.write_byte(0x4A, 0x00)?;
523
524        self.write_byte(0xFF, 0x00)?;
525        self.write_byte(0x7A, 0x0A)?;
526        self.write_byte(0x7B, 0x00)?;
527        self.write_byte(0x78, 0x21)?;
528
529        self.write_byte(0xFF, 0x01)?;
530        self.write_byte(0x23, 0x34)?;
531        self.write_byte(0x42, 0x00)?;
532        self.write_byte(0x44, 0xFF)?;
533        self.write_byte(0x45, 0x26)?;
534        self.write_byte(0x46, 0x05)?;
535        self.write_byte(0x40, 0x40)?;
536        self.write_byte(0x0E, 0x06)?;
537        self.write_byte(0x20, 0x1A)?;
538        self.write_byte(0x43, 0x40)?;
539
540        self.write_byte(0xFF, 0x00)?;
541        self.write_byte(0x34, 0x03)?;
542        self.write_byte(0x35, 0x44)?;
543
544        self.write_byte(0xFF, 0x01)?;
545        self.write_byte(0x31, 0x04)?;
546        self.write_byte(0x4B, 0x09)?;
547        self.write_byte(0x4C, 0x05)?;
548        self.write_byte(0x4D, 0x04)?;
549
550        self.write_byte(0xFF, 0x00)?;
551        self.write_byte(0x44, 0x00)?;
552        self.write_byte(0x45, 0x20)?;
553        self.write_byte(0x47, 0x08)?;
554        self.write_byte(0x48, 0x28)?;
555        self.write_byte(0x67, 0x00)?;
556        self.write_byte(0x70, 0x04)?;
557        self.write_byte(0x71, 0x01)?;
558        self.write_byte(0x72, 0xFE)?;
559        self.write_byte(0x76, 0x00)?;
560        self.write_byte(0x77, 0x00)?;
561
562        self.write_byte(0xFF, 0x01)?;
563        self.write_byte(0x0D, 0x01)?;
564
565        self.write_byte(0xFF, 0x00)?;
566        self.write_byte(0x80, 0x01)?;
567        self.write_byte(0x01, 0xF8)?;
568
569        self.write_byte(0xFF, 0x01)?;
570        self.write_byte(0x8E, 0x01)?;
571        self.write_byte(0x00, 0x01)?;
572        self.write_byte(0xFF, 0x00)?;
573        self.write_byte(0x80, 0x00)?;
574
575        // -- VL53L0X_load_tuning_settings() end
576
577        // "Set interrupt config to new sample ready"
578        // -- VL53L0X_SetGpioConfig() begin
579
580        self.write_register(Register::SYSTEM_INTERRUPT_CONFIG_GPIO, 0x04)?;
581        // active low
582        let high = self.read_register(Register::GPIO_HV_MUX_ACTIVE_HIGH)?;
583        self.write_register(Register::GPIO_HV_MUX_ACTIVE_HIGH, high & !0x10)?;
584        self.write_register(Register::SYSTEM_INTERRUPT_CLEAR, 0x01)?;
585
586        // -- VL53L0X_SetGpioConfig() end
587        // "Disable MSRC and TCC by default"
588        // MSRC = Minimum Signal Rate Check
589        // TCC = Target CentreCheck
590        // -- VL53L0X_SetSequenceStepEnable() begin
591        self.measurement_timing_budget_microseconds =
592            self.get_measurement_timing_budget()?;
593        self.write_register(Register::SYSTEM_SEQUENCE_CONFIG, 0xE8)?;
594
595        // -- VL53L0X_SetSequenceStepEnable() end
596
597        // "Recalculate timing budget"
598        let mtbm = self.measurement_timing_budget_microseconds;
599        self.set_measurement_timing_budget(mtbm)?;
600
601        // VL53L0X_StaticInit() end
602
603        // VL53L0X_PerformRefCalibration() begin (VL53L0X_perform_ref_calibration())
604
605        // -- VL53L0X_perform_vhv_calibration() begin
606
607        self.write_register(Register::SYSTEM_SEQUENCE_CONFIG, 0x01)?;
608        self.perform_single_ref_calibration(0x40)?;
609        // -- VL53L0X_perform_vhv_calibration() end
610        // -- VL53L0X_perform_phase_calibration() begin
611
612        self.write_register(Register::SYSTEM_SEQUENCE_CONFIG, 0x02)?;
613        self.perform_single_ref_calibration(0x00)?;
614
615        // -- VL53L0X_perform_phase_calibration() end
616
617        // "restore the previous Sequence Config"
618        self.write_register(Register::SYSTEM_SEQUENCE_CONFIG, 0xE8)?;
619
620        // VL53L0X_PerformRefCalibration() end
621        Ok(())
622    }
623
624    /// Returns who am i
625    pub fn who_am_i(&mut self) -> Result<u8, E> {
626        self.read_register(Register::WHO_AM_I)
627    }
628
629    fn get_vcsel_pulse_period(&mut self, ty: VcselPeriodType) -> Result<u8, E> {
630        match ty {
631            VcselPeriodType::VcselPeriodPreRange => Ok(decode_vcsel_period(
632                self.read_register(Register::PRE_RANGE_CONFIG_VCSEL_PERIOD)?,
633            )),
634            VcselPeriodType::VcselPeriodFinalRange => Ok(decode_vcsel_period(
635                self.read_register(Register::FINAL_RANGE_CONFIG_VCSEL_PERIOD)?,
636            )),
637        }
638    }
639
640    // getSequenceStepEnables(VL53L0XSequenceStepEnables* enables) {
641    fn get_sequence_step_enables(&mut self) -> Result<SeqStepEnables, E> {
642        let sequence_config: u8 =
643            self.read_register(Register::SYSTEM_SEQUENCE_CONFIG)?;
644        Ok(SeqStepEnables {
645            tcc: ((sequence_config >> 4) & 0x1) == 1,
646            dss: ((sequence_config >> 3) & 0x1) == 1,
647            msrc: ((sequence_config >> 2) & 0x1) == 1,
648            pre_range: ((sequence_config >> 6) & 0x1) == 1,
649            final_range: ((sequence_config >> 7) & 0x1) == 1,
650        })
651    }
652
653    // getSequenceStepTimeouts(timeouts)
654    fn get_sequence_step_timeouts(
655        &mut self,
656        enables: &SeqStepEnables,
657    ) -> Result<SeqStepTimeouts, E> {
658        let pre_range_mclks = decode_timeout(
659            self.read_16bit(Register::PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI)?,
660        );
661        let mut final_range_mclks = decode_timeout(
662            self.read_16bit(Register::FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI)?,
663        );
664        if enables.pre_range {
665            final_range_mclks -= pre_range_mclks;
666        };
667        let pre_range_vcselperiod_pclks =
668            self.get_vcsel_pulse_period(VcselPeriodType::VcselPeriodPreRange)?;
669        let msrc_dss_tcc_mclks =
670            self.read_register(Register::MSRC_CONFIG_TIMEOUT_MACROP)? + 1;
671        let final_range_vcsel_period_pclks = self
672            .get_vcsel_pulse_period(VcselPeriodType::VcselPeriodFinalRange)?;
673        Ok(SeqStepTimeouts {
674            pre_range_vcselperiod_pclks,
675            msrc_dss_tcc_mclks,
676            msrc_dss_tcc_microseconds: timeout_mclks_to_microseconds(
677                msrc_dss_tcc_mclks as u16,
678                pre_range_vcselperiod_pclks,
679            ),
680            pre_range_mclks: pre_range_mclks,
681            pre_range_microseconds: timeout_mclks_to_microseconds(
682                pre_range_mclks,
683                pre_range_vcselperiod_pclks,
684            ),
685            final_range_mclks,
686            final_range_vcsel_period_pclks,
687            final_range_microseconds: timeout_mclks_to_microseconds(
688                final_range_mclks,
689                final_range_vcsel_period_pclks,
690            ),
691        })
692    }
693
694    // uint32_t VL53L0X::getMeasurementTimingBudget() {
695    fn get_measurement_timing_budget(&mut self) -> Result<u32, E> {
696        let start_overhead: u32 = 1910;
697        let end_overhead: u32 = 960;
698        let msrc_overhead: u32 = 660;
699        let tcc_overhead: u32 = 590;
700        let dss_overhead: u32 = 690;
701        let pre_range_overhead: u32 = 660;
702        let final_range_overhead: u32 = 550;
703
704        let enables = self.get_sequence_step_enables()?;
705        let timeouts = self.get_sequence_step_timeouts(&enables)?;
706
707        // "Start and end overhead times always present"
708        let mut budget_microseconds = start_overhead + end_overhead;
709        if enables.tcc {
710            budget_microseconds +=
711                timeouts.msrc_dss_tcc_microseconds + tcc_overhead;
712        }
713        if enables.dss {
714            budget_microseconds +=
715                2 * (timeouts.msrc_dss_tcc_microseconds + dss_overhead);
716        } else if enables.msrc {
717            budget_microseconds +=
718                timeouts.msrc_dss_tcc_microseconds + msrc_overhead;
719        }
720        if enables.pre_range {
721            budget_microseconds +=
722                timeouts.pre_range_microseconds + pre_range_overhead;
723        }
724        if enables.final_range {
725            budget_microseconds +=
726                timeouts.final_range_microseconds + final_range_overhead;
727        }
728
729        // store for internal reuse
730        Ok(budget_microseconds)
731    }
732
733    /// setMeasurementTimingBudget(budget_microseconds)
734    pub fn set_measurement_timing_budget(
735        &mut self,
736        budget_microseconds: u32,
737    ) -> Result<bool, E> {
738        // note that these are different than values in get_
739        let start_overhead: u32 = 1320;
740        let end_overhead: u32 = 960;
741        let msrc_overhead: u32 = 660;
742        let tcc_overhead: u32 = 590;
743        let dss_overhead: u32 = 690;
744        let pre_range_overhead: u32 = 660;
745        let final_range_overhead: u32 = 550;
746        let min_timing_budget: u32 = 20000;
747
748        if budget_microseconds < min_timing_budget {
749            return Ok(false);
750        }
751
752        let enables = self.get_sequence_step_enables()?;
753        let timeouts = self.get_sequence_step_timeouts(&enables)?;
754
755        let mut use_budget_microseconds: u32 =
756            (start_overhead + end_overhead) as u32;
757        if enables.tcc {
758            use_budget_microseconds +=
759                timeouts.msrc_dss_tcc_microseconds + tcc_overhead;
760        }
761        if enables.dss {
762            use_budget_microseconds +=
763                2 * timeouts.msrc_dss_tcc_microseconds + dss_overhead;
764        } else if enables.msrc {
765            use_budget_microseconds +=
766                timeouts.msrc_dss_tcc_microseconds + msrc_overhead;
767        }
768        if enables.pre_range {
769            use_budget_microseconds +=
770                timeouts.pre_range_microseconds + pre_range_overhead;
771        }
772        if enables.final_range {
773            use_budget_microseconds += final_range_overhead;
774        }
775
776        // "Note that the final range timeout is determined by the timing
777        // budget and the sum of all other timeouts within the sequence.
778        // If there is no room for the final range timeout, then an error
779        // will be set. Otherwise the remaining time will be applied to
780        // the final range."
781
782        if use_budget_microseconds > budget_microseconds {
783            // "Requested timeout too small."
784            return Ok(false);
785        }
786
787        let final_range_timeout_microseconds: u32 =
788            budget_microseconds - use_budget_microseconds;
789
790        // set_sequence_step_timeout() begin
791        // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE)
792        // "For the final range timeout, the pre-range timeout
793        // must be added. To do this both final and pre-range
794        // timeouts must be expressed in macro periods MClks
795        // because they have different vcsel periods."
796        let mut final_range_timeout_mclks: u16 = timeout_microseconds_to_mclks(
797            final_range_timeout_microseconds,
798            timeouts.final_range_vcsel_period_pclks,
799        ) as u16;
800
801        if enables.pre_range {
802            final_range_timeout_mclks += timeouts.pre_range_mclks;
803        }
804
805        self.write_16bit(
806            Register::FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI,
807            encode_timeout(final_range_timeout_mclks),
808        )?;
809
810        // set_sequence_step_timeout() end
811        // store for internal reuse
812        self.measurement_timing_budget_microseconds = budget_microseconds;
813        Ok(true)
814    }
815
816    /*
817        fn write_byte_raw(&mut self, reg: u8, byte: u8) {
818            // FIXME:
819            //  * remove this function
820            //  * device address is not a const
821            //  * register address is u16
822            let mut buffer = [0];
823            let _ = self.com.write_read(ADDRESS, &[reg, byte], &mut buffer);
824        }
825
826        fn read_byte_raw(&mut self, reg: u8) -> u8 {
827            // FIXME:
828            //  * remove this function
829            //  * device address is not a const
830            //  * register address is u16
831            let mut data: [u8; 1] = [0];
832            let _ = self.com.write_read(ADDRESS, &[reg], &mut data);
833            data[0]
834        }
835
836        fn write_byte(&mut self, reg: Register, byte: u8) {
837            let mut buffer = [0];
838            let _ = self
839                .com
840                .write_read(ADDRESS, &[reg as u8, byte], &mut buffer);
841        }
842    */
843}
844
845struct SeqStepEnables {
846    tcc: bool,
847    dss: bool,
848    msrc: bool,
849    pre_range: bool,
850    final_range: bool,
851}
852
853struct SeqStepTimeouts {
854    pre_range_vcselperiod_pclks: u8,
855    final_range_vcsel_period_pclks: u8,
856    msrc_dss_tcc_mclks: u8,
857    pre_range_mclks: u16,
858    final_range_mclks: u16,
859    msrc_dss_tcc_microseconds: u32,
860    pre_range_microseconds: u32,
861    final_range_microseconds: u32,
862}
863
864fn decode_timeout(register_value: u16) -> u16 {
865    // format: "(LSByte * 2^MSByte) + 1"
866    ((register_value & 0x00FF) << (((register_value & 0xFF00) as u16) >> 8))
867        as u16
868        + 1
869}
870
871fn encode_timeout(timeout_mclks: u16) -> u16 {
872    if timeout_mclks == 0 {
873        return 0;
874    }
875    let mut ls_byte: u32;
876    let mut ms_byte: u16 = 0;
877
878    ls_byte = (timeout_mclks as u32) - 1;
879
880    while (ls_byte & 0xFFFFFF00) > 0 {
881        ls_byte >>= 1;
882        ms_byte += 1;
883    }
884
885    return (ms_byte << 8) | ((ls_byte & 0xFF) as u16);
886}
887
888fn calc_macro_period(vcsel_period_pclks: u8) -> u32 {
889    ((2304u32 * (vcsel_period_pclks as u32) * 1655u32) + 500u32) / 1000u32
890}
891
892fn timeout_mclks_to_microseconds(
893    timeout_period_mclks: u16,
894    vcsel_period_pclks: u8,
895) -> u32 {
896    let macro_period_nanoseconds: u32 =
897        calc_macro_period(vcsel_period_pclks) as u32;
898    (((timeout_period_mclks as u32) * macro_period_nanoseconds)
899        + (macro_period_nanoseconds / 2))
900        / 1000
901}
902
903fn timeout_microseconds_to_mclks(
904    timeout_period_microseconds: u32,
905    vcsel_period_pclks: u8,
906) -> u32 {
907    let macro_period_nanoseconds: u32 =
908        calc_macro_period(vcsel_period_pclks) as u32;
909
910    ((timeout_period_microseconds * 1000) + (macro_period_nanoseconds / 2))
911        / macro_period_nanoseconds
912}
913
914// Decode VCSEL (vertical cavity surface emitting laser) pulse period in PCLKs from register value based on VL53L0X_decode_vcsel_period()
915fn decode_vcsel_period(register_value: u8) -> u8 {
916    ((register_value) + 1) << 1
917}
918
919// Encode VCSEL pulse period register value from period in PCLKs based on VL53L0X_encode_vcsel_period()
920fn encode_vcsel_period(period_pclks: u8) -> u8 {
921    ((period_pclks) >> 1) - 1
922}
923
924#[allow(non_camel_case_types)]
925enum Register {
926    SYSRANGE_START = 0x00,
927    WHO_AM_I = 0xC0,
928    VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV = 0x89,
929    MSRC_CONFIG_CONTROL = 0x60,
930    SYSTEM_SEQUENCE_CONFIG = 0x01,
931    FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT = 0x44,
932    GLOBAL_CONFIG_SPAD_ENABLES_REF_0 = 0xB0,
933    DYNAMIC_SPAD_REF_EN_START_OFFSET = 0x4F,
934    DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD = 0x4E,
935    GLOBAL_CONFIG_REF_EN_START_SELECT = 0xB6,
936    SYSTEM_INTERRUPT_CONFIG_GPIO = 0x0A,
937    GPIO_HV_MUX_ACTIVE_HIGH = 0x84,
938    SYSTEM_INTERRUPT_CLEAR = 0x0B,
939    RESULT_INTERRUPT_STATUS = 0x13,
940    RESULT_RANGE_STATUS = 0x14,
941    RESULT_RANGE_STATUS_plus_10 = 0x1e,
942    OSC_CALIBRATE_VAL = 0xF8,
943    SYSTEM_INTERMEASUREMENT_PERIOD = 0x04,
944    FINAL_RANGE_CONFIG_VCSEL_PERIOD = 0x70,
945    PRE_RANGE_CONFIG_VCSEL_PERIOD = 0x50,
946    PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x51,
947    PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x52,
948    FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x71,
949    FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x72,
950    CROSSTALK_COMPENSATION_PEAK_RATE_MCPS = 0x20,
951    MSRC_CONFIG_TIMEOUT_MACROP = 0x46,
952    I2C_SLAVE_DEVICE_ADDRESS = 0x8A,
953}
954
955#[derive(Debug, Copy, Clone)]
956enum VcselPeriodType {
957    VcselPeriodPreRange = 0,
958    VcselPeriodFinalRange = 1,
959}
vl53l0x/lib.rs

vl53l0x/
lib.rs
