1#![no_std]
87#![warn(missing_docs)]
88
89use cfg_if::cfg_if;
90use comm::{Read, Write};
91use core::fmt::Debug;
92use roi::{ROICenter, ROI};
93use threshold::{Threshold, Window};
94pub mod comm;
95pub mod roi;
96pub mod threshold;
97
98#[derive(Debug, PartialEq, Eq, Clone, Copy)]
100pub struct SWVersion {
101 pub major: u8,
103 pub minor: u8,
105 pub build: u8,
107 pub revision: u32,
109}
110
111#[derive(Debug)]
113pub enum Error<E>
114where
115 E: Debug,
116{
117 CommunicationError(E),
119 InvalidTimingBudget,
121 InvalidDistanceMode,
123 InvalidSigmaThreshold,
125}
126
127impl<E> From<E> for Error<E>
128where
129 E: Debug,
130{
131 fn from(e: E) -> Self {
132 Self::CommunicationError(e)
133 }
134}
135
136#[derive(Debug, PartialEq, Eq, Clone, Copy)]
138pub enum Polarity {
139 ActiveHigh = 1,
141 ActiveLow = 0,
143}
144
145impl From<u8> for Polarity {
146 fn from(v: u8) -> Self {
147 match v {
148 0 => Polarity::ActiveHigh,
149 _ => Polarity::ActiveLow,
150 }
151 }
152}
153
154#[derive(Debug, PartialEq, Eq, Clone, Copy)]
156pub enum DistanceMode {
157 Short = 1,
160 Long = 2,
163}
164
165#[derive(Debug, PartialEq, Eq, Clone, Copy)]
167pub enum RangeStatus {
168 Valid = 0,
170 SigmaFailure = 1,
172 SignalFailure = 2,
174 MinRangeClipped = 3,
176 OutOfBounds = 4,
178 HardwareFailure = 5,
180 WrapCheckFail = 6,
182 Wraparound = 7,
184 ProcessingFailure = 8,
186 CrosstalkSignal = 9,
188 Synchronisation = 10,
190 MergedPulse = 11,
192 LackOfSignal = 12,
194 MinRangeFail = 13,
196 InvalidRange = 14,
198 None = 255,
200}
201
202impl From<u8> for RangeStatus {
203 fn from(v: u8) -> Self {
204 match v {
205 3 => RangeStatus::HardwareFailure,
206 4 => RangeStatus::SignalFailure,
207 5 => RangeStatus::OutOfBounds,
208 6 => RangeStatus::SigmaFailure,
209 7 => RangeStatus::Wraparound,
210 8 => RangeStatus::MinRangeClipped,
211 9 => RangeStatus::Valid,
212 12 => RangeStatus::CrosstalkSignal,
213 13 => RangeStatus::MinRangeFail,
214 18 => RangeStatus::Synchronisation,
215 19 => RangeStatus::WrapCheckFail,
216 22 => RangeStatus::MergedPulse,
217 23 => RangeStatus::LackOfSignal,
218 _ => RangeStatus::None,
219 }
220 }
221}
222
223#[derive(Debug, PartialEq, Eq, Clone, Copy)]
225pub enum IOVoltage {
226 Volt1_8,
228 Volt2_8,
230}
231
232#[derive(Debug, PartialEq, Eq, Clone, Copy)]
234pub struct MeasureResult {
235 pub status: RangeStatus,
237 pub distance_mm: u16,
239 pub ambient: u16,
241 pub sig_per_spad: u16,
243 pub spad_count: u16,
245}
246
247pub const DEFAULT_ADDRESS: u8 = 0x29;
249
250pub use vl53l1_reg::Index as Register;
251
252pub struct VL53L1X<T>
254where
255 T: Write + Read,
256 {
259 i2c: T,
260 address: u8,
261}
262
263impl<T, E> VL53L1X<T>
264where
265 E: Debug,
266 T: Write<Error = E> + Read<Error = E>,
267{
268 pub fn new(i2c: T, address: u8) -> VL53L1X<T> {
275 VL53L1X { i2c, address }
276 }
277
278 pub fn sw_version() -> SWVersion {
280 SWVersion {
281 major: 3,
282 minor: 5,
283 build: 1,
284 revision: 0,
285 }
286 }
287
288 pub const DEFAULT_CONFIG: [u8; 88] = [
290 0x01, 0x02, 0x00, 0x02, 0x08, 0x00, 0x08, 0x10, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0xff, 0x00, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x0b, 0x00, 0x00, 0x02, 0x0a, 0x21, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x00, 0x38, 0xff, 0x01, 0x00, 0x08, 0x00, 0x00, 0x01, 0xcc, 0x0f, 0x01, 0xf1, 0x0d, 0x01, 0x68, 0x00, 0x80, 0x08, 0xb8, 0x00, 0x00, 0x00, 0x00, 0x0f, 0x89, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x0f, 0x0d, 0x0e, 0x0e, 0x00, 0x00, 0x02, 0xc7, 0xff, 0x9B, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, ];
379
380 pub fn write_bytes<R>(&mut self, address: R, bytes: &[u8]) -> Result<(), Error<E>>
388 where
389 R: Into<[u8; 2]>,
390 {
391 self.i2c
392 .write_registers(self.address, address.into(), bytes)?;
393
394 Ok(())
395 }
396
397 pub fn read_bytes<R>(&mut self, address: R, bytes: &mut [u8]) -> Result<(), Error<E>>
404 where
405 R: Into<[u8; 2]>,
406 {
407 self.i2c
408 .read_registers(self.address, address.into(), bytes)?;
409
410 Ok(())
411 }
412
413 pub fn set_address(&mut self, new_address: u8) -> Result<(), Error<E>> {
419 self.write_bytes(Register::I2C_SLAVE__DEVICE_ADDRESS, &[new_address])?;
420
421 self.address = new_address;
422
423 Ok(())
424 }
425
426 pub fn init(&mut self, io_config: IOVoltage) -> Result<(), Error<E>> {
432 self.write_bytes(Register::PAD_I2C_HV__CONFIG, &[0])?;
433
434 let io = match io_config {
435 IOVoltage::Volt1_8 => 0,
436 IOVoltage::Volt2_8 => 1,
437 };
438
439 self.write_bytes(Register::GPIO_HV_PAD__CTRL, &[io])?;
440 self.write_bytes(Register::PAD_I2C_HV__EXTSUP_CONFIG, &[io])?;
441
442 cfg_if! {
443 if #[cfg(feature = "i2c-iter")] {
444 self.write_bytes(Register::GPIO_HV_MUX__CTRL, &Self::DEFAULT_CONFIG)?;
445 } else {
446 for (byte, address) in Self::DEFAULT_CONFIG.iter().zip(0x30u16..0x88) {
447 self.write_bytes(address.to_be_bytes(), &[*byte])?;
448 }
449 }
450 }
451
452 self.start_ranging()?;
453
454 while !self.is_data_ready()? {}
455
456 self.clear_interrupt()?;
457 self.stop_ranging()?;
458
459 self.write_bytes(Register::VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, &[0x09])?;
460 self.write_bytes(Register::VHV_CONFIG__INIT, &[0])?;
461
462 Ok(())
463 }
464
465 pub fn clear_interrupt(&mut self) -> Result<(), Error<E>> {
468 self.write_bytes(Register::SYSTEM__INTERRUPT_CLEAR, &[0x01])
469 }
470
471 pub fn set_interrupt_polarity(&mut self, polarity: Polarity) -> Result<(), Error<E>> {
477 let mut gpio_mux_hv = [0u8];
478
479 self.read_bytes(Register::GPIO_HV_MUX__CTRL, &mut gpio_mux_hv)?;
480
481 gpio_mux_hv[0] &= 0xEF;
482
483 gpio_mux_hv[0] |= (polarity as u8) << 4;
484
485 self.write_bytes(Register::GPIO_HV_MUX__CTRL, &gpio_mux_hv)
486 }
487
488 pub fn get_interrupt_polarity(&mut self) -> Result<Polarity, Error<E>> {
490 let mut gpio_mux_hv = [0u8];
491
492 self.read_bytes(Register::GPIO_HV_MUX__CTRL, &mut gpio_mux_hv)?;
493
494 Ok((gpio_mux_hv[0] & 0x10).into())
495 }
496
497 pub fn start_ranging(&mut self) -> Result<(), Error<E>> {
500 self.write_bytes(Register::SYSTEM__MODE_START, &[0x40])
501 }
502
503 pub fn stop_ranging(&mut self) -> Result<(), Error<E>> {
505 self.write_bytes(Register::SYSTEM__MODE_START, &[0x00])
506 }
507
508 pub fn is_data_ready(&mut self) -> Result<bool, Error<E>> {
510 let polarity = self.get_interrupt_polarity()? as u8;
511
512 let mut state = [0u8];
513
514 self.read_bytes(Register::GPIO__TIO_HV_STATUS, &mut state)?;
515
516 if (state[0] & 0x01) == polarity {
517 return Ok(true);
518 }
519
520 Ok(false)
521 }
522
523 pub fn set_timing_budget_ms(&mut self, milliseconds: u16) -> Result<(), Error<E>> {
529 let mode = self.get_distance_mode()?;
530
531 let (a, b) = match mode {
532 DistanceMode::Short => match milliseconds {
533 15 => (0x01Du16, 0x0027u16),
534 20 => (0x051, 0x006E),
535 33 => (0x00D6, 0x006E),
536 50 => (0x1AE, 0x01E8),
537 100 => (0x02E1, 0x0388),
538 200 => (0x03E1, 0x0496),
539 500 => (0x0591, 0x05C1),
540 _ => return Err(Error::InvalidTimingBudget),
541 },
542 DistanceMode::Long => match milliseconds {
543 20 => (0x001E, 0x0022),
544 33 => (0x0060, 0x006E),
545 50 => (0x00AD, 0x00C6),
546 100 => (0x01CC, 0x01EA),
547 200 => (0x02D9, 0x02F8),
548 500 => (0x048F, 0x04A4),
549 _ => return Err(Error::InvalidTimingBudget),
550 },
551 };
552
553 self.write_bytes(
554 Register::RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
555 &a.to_be_bytes(),
556 )?;
557 self.write_bytes(
558 Register::RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
559 &b.to_be_bytes(),
560 )?;
561
562 Ok(())
563 }
564
565 pub fn get_timing_budget_ms(&mut self) -> Result<u16, Error<E>> {
567 let mut a = [0u8, 0];
568
569 self.read_bytes(Register::RANGE_CONFIG__TIMEOUT_MACROP_A_HI, &mut a)?;
570
571 Ok(match u16::from_be_bytes(a) {
572 0x001D => 15,
573 0x0051 | 0x001E => 20,
574 0x00D6 | 0x0060 => 33,
575 0x1AE | 0x00AD => 50,
576 0x02E1 | 0x01CC => 100,
577 0x03E1 | 0x02D9 => 200,
578 0x0591 | 0x048F => 500,
579 _ => return Err(Error::InvalidTimingBudget),
580 })
581 }
582
583 pub fn set_distance_mode(&mut self, mode: DistanceMode) -> Result<(), Error<E>> {
589 let tb = self.get_timing_budget_ms()?;
590
591 let (timeout, vcsel_a, vcsel_b, phase, woi_sd0, phase_sd0) = match mode {
592 DistanceMode::Short => (0x14u8, 0x07u8, 0x05u8, 0x38u8, 0x0705u16, 0x0606u16),
593 DistanceMode::Long => (0x0A, 0x0F, 0x0D, 0xB8, 0x0F0D, 0x0E0E),
594 };
595
596 self.write_bytes(
597 Register::PHASECAL_CONFIG__TIMEOUT_MACROP,
598 &timeout.to_be_bytes(),
599 )?;
600 self.write_bytes(
601 Register::RANGE_CONFIG__VCSEL_PERIOD_A,
602 &vcsel_a.to_be_bytes(),
603 )?;
604 self.write_bytes(
605 Register::RANGE_CONFIG__VCSEL_PERIOD_B,
606 &vcsel_b.to_be_bytes(),
607 )?;
608 self.write_bytes(
609 Register::RANGE_CONFIG__VALID_PHASE_HIGH,
610 &phase.to_be_bytes(),
611 )?;
612 self.write_bytes(Register::SD_CONFIG__WOI_SD0, &woi_sd0.to_be_bytes())?;
613 self.write_bytes(
614 Register::SD_CONFIG__INITIAL_PHASE_SD0,
615 &phase_sd0.to_be_bytes(),
616 )?;
617
618 self.set_timing_budget_ms(tb)?;
619
620 Ok(())
621 }
622
623 pub fn get_distance_mode(&mut self) -> Result<DistanceMode, Error<E>> {
625 let mut out = [0u8];
626 self.read_bytes(Register::PHASECAL_CONFIG__TIMEOUT_MACROP, &mut out)?;
627
628 if out[0] == 0x14 {
629 Ok(DistanceMode::Short)
630 } else if out[0] == 0x0A {
631 Ok(DistanceMode::Long)
632 } else {
633 Err(Error::InvalidDistanceMode)
634 }
635 }
636
637 pub fn set_inter_measurement_period_ms(&mut self, milliseconds: u16) -> Result<(), Error<E>> {
644 let mut clock_pll = [0u8, 0];
645
646 self.read_bytes(Register::RESULT__OSC_CALIBRATE_VAL, &mut clock_pll)?;
647
648 let clock_pll = u16::from_be_bytes(clock_pll) & 0x3FF;
649
650 let val = ((clock_pll * milliseconds) as f32 * 1.075f32) as u32;
651
652 self.write_bytes(
653 Register::SYSTEM__INTERMEASUREMENT_PERIOD,
654 &val.to_be_bytes(),
655 )?;
656
657 Ok(())
658 }
659
660 pub fn get_inter_measurement_period_ms(&mut self) -> Result<u16, Error<E>> {
662 let mut clock_pll = [0u8, 0];
663 let mut period = [0u8, 0, 0, 0];
664
665 self.read_bytes(Register::RESULT__OSC_CALIBRATE_VAL, &mut clock_pll)?;
666
667 let clock_pll = u16::from_be_bytes(clock_pll) & 0x3FF;
668
669 self.read_bytes(Register::SYSTEM__INTERMEASUREMENT_PERIOD, &mut period)?;
670
671 let period = u32::from_be_bytes(period);
672
673 Ok((period / (clock_pll as f32 * 1.065f32) as u32) as u16)
674 }
675
676 pub fn is_booted(&mut self) -> Result<bool, Error<E>> {
678 let mut status = [0u8];
679 self.read_bytes(Register::FIRMWARE__SYSTEM_STATUS, &mut status)?;
680
681 Ok(status[0] == 1)
682 }
683
684 pub fn get_sensor_id(&mut self) -> Result<u16, Error<E>> {
686 let mut id = [0u8, 0];
687
688 self.read_bytes(Register::IDENTIFICATION__MODEL_ID, &mut id)?;
689
690 Ok(u16::from_be_bytes(id))
691 }
692
693 pub fn get_distance(&mut self) -> Result<u16, Error<E>> {
695 let mut distance = [0u8, 0];
696
697 self.read_bytes(
698 Register::RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0,
699 &mut distance,
700 )?;
701
702 Ok(u16::from_be_bytes(distance))
703 }
704
705 pub fn get_signal_per_spad(&mut self) -> Result<u16, Error<E>> {
707 let mut signal = [0, 0];
708 let mut spad_count = [0, 0];
709
710 self.read_bytes(
711 Register::RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0,
712 &mut signal,
713 )?;
714 self.read_bytes(
715 Register::RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0,
716 &mut spad_count,
717 )?;
718
719 let signal = u16::from_be_bytes(signal);
720 let spad_count = u16::from_be_bytes(spad_count);
721
722 Ok((200.0f32 * signal as f32 / spad_count as f32).min(u16::MAX as f32) as u16)
723 }
724
725 pub fn get_ambient_per_spad(&mut self) -> Result<u16, Error<E>> {
727 let mut spad_count = [0, 0];
728 let mut ambient = [0, 0];
729
730 self.read_bytes(
731 Register::RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0,
732 &mut spad_count,
733 )?;
734 self.read_bytes(Register::RESULT__AMBIENT_COUNT_RATE_MCPS_SD0, &mut ambient)?;
735
736 let spad_count = u16::from_be_bytes(spad_count);
737 let ambient = u16::from_be_bytes(ambient);
738
739 Ok((200.0f32 * ambient as f32 / spad_count as f32).min(u16::MAX as f32) as u16)
740 }
741
742 pub fn get_signal_rate(&mut self) -> Result<u16, Error<E>> {
744 let mut signal = [0, 0];
745
746 self.read_bytes(
747 Register::RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0,
748 &mut signal,
749 )?;
750
751 Ok(u16::from_be_bytes(signal).saturating_mul(8))
752 }
753
754 pub fn get_spad_count(&mut self) -> Result<u16, Error<E>> {
756 let mut spad_count = [0, 0];
757
758 self.read_bytes(
759 Register::RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0,
760 &mut spad_count,
761 )?;
762
763 Ok(u16::from_be_bytes(spad_count) >> 8)
764 }
765
766 pub fn get_ambient_rate(&mut self) -> Result<u16, Error<E>> {
768 let mut ambient = [0, 0];
769
770 self.read_bytes(Register::RESULT__AMBIENT_COUNT_RATE_MCPS_SD0, &mut ambient)?;
771
772 Ok(u16::from_be_bytes(ambient).saturating_mul(8))
773 }
774
775 pub fn get_range_status(&mut self) -> Result<RangeStatus, Error<E>> {
777 let mut status = [0];
778
779 self.read_bytes(Register::RESULT__RANGE_STATUS, &mut status)?;
780
781 let status = u8::from_be_bytes(status) & 0x1F;
782
783 Ok(status.into())
784 }
785
786 pub fn get_result(&mut self) -> Result<MeasureResult, Error<E>> {
788 let mut result = [0; 17];
789
790 self.read_bytes(Register::RESULT__RANGE_STATUS, &mut result)?;
791
792 Ok(MeasureResult {
793 status: (result[0] & 0x1F).into(),
794 ambient: u16::from_be_bytes([result[7], result[8]]).saturating_mul(8),
795 spad_count: result[3] as u16,
796 sig_per_spad: u16::from_be_bytes([result[15], result[16]]).saturating_mul(8),
797 distance_mm: u16::from_be_bytes([result[13], result[14]]),
798 })
799 }
800
801 pub fn set_offset(&mut self, offset: i16) -> Result<(), Error<E>> {
807 self.write_bytes(
808 Register::ALGO__PART_TO_PART_RANGE_OFFSET_MM,
809 &(offset.saturating_mul(4)).to_be_bytes(),
810 )?;
811
812 self.write_bytes(Register::MM_CONFIG__INNER_OFFSET_MM, &[0, 0])?;
813 self.write_bytes(Register::MM_CONFIG__OUTER_OFFSET_MM, &[0, 0])?;
814
815 Ok(())
816 }
817
818 pub fn get_offset(&mut self) -> Result<i16, Error<E>> {
820 let mut offset = [0, 0];
821
822 self.read_bytes(Register::ALGO__PART_TO_PART_RANGE_OFFSET_MM, &mut offset)?;
823
824 let mut offset = u16::from_be_bytes(offset) << 3;
825
826 offset >>= 5;
827
828 Ok(offset as i16)
829 }
830
831 pub fn set_cross_talk(&mut self, correction: u16) -> Result<(), Error<E>> {
837 self.write_bytes(
838 Register::ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS,
839 &[0, 0],
840 )?;
841 self.write_bytes(
842 Register::ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS,
843 &[0, 0],
844 )?;
845
846 let correction = (correction << 9) / 1000;
847
848 self.write_bytes(
849 Register::ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS,
850 &correction.to_be_bytes(),
851 )?;
852
853 Ok(())
854 }
855
856 pub fn get_cross_talk(&mut self) -> Result<u16, Error<E>> {
858 let mut correction = [0, 0];
859
860 self.read_bytes(
861 Register::ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS,
862 &mut correction,
863 )?;
864
865 let correction =
866 ((u16::from_be_bytes(correction) as u32 * 1000) >> 9).min(u16::MAX as u32) as u16;
867
868 Ok(correction)
869 }
870
871 pub fn set_distance_threshold(&mut self, threshold: Threshold) -> Result<(), Error<E>> {
877 let mut config = [0];
878
879 self.read_bytes(Register::SYSTEM__INTERRUPT_CONFIG_GPIO, &mut config)?;
880
881 let config = config[0] & 0x47 | (threshold.window as u8 & 0x07);
882
883 self.write_bytes(Register::SYSTEM__INTERRUPT_CONFIG_GPIO, &[config])?;
884 self.write_bytes(Register::SYSTEM__THRESH_HIGH, &threshold.high.to_be_bytes())?;
885 self.write_bytes(Register::SYSTEM__THRESH_LOW, &threshold.low.to_be_bytes())?;
886
887 Ok(())
888 }
889
890 pub fn get_distance_threshold(&mut self) -> Result<Threshold, Error<E>> {
892 let mut config = [0];
893
894 self.read_bytes(Register::SYSTEM__INTERRUPT_CONFIG_GPIO, &mut config)?;
895
896 let window: Window = (config[0] & 0x07).into();
897
898 let mut high = [0, 0];
899 let mut low = [0, 0];
900
901 self.read_bytes(Register::SYSTEM__THRESH_HIGH, &mut high)?;
902 self.read_bytes(Register::SYSTEM__THRESH_LOW, &mut low)?;
903
904 Ok(Threshold {
905 window,
906 low: u16::from_be_bytes(low),
907 high: u16::from_be_bytes(high),
908 })
909 }
910
911 pub fn set_roi(&mut self, mut roi: ROI) -> Result<(), Error<E>> {
918 debug_assert!(roi.width >= 4);
919 debug_assert!(roi.height >= 4);
920
921 let mut center = [0];
922
923 self.read_bytes(Register::ROI_CONFIG__MODE_ROI_CENTRE_SPAD, &mut center)?;
924
925 if roi.width > 16 {
926 roi.width = 16;
927 }
928
929 if roi.height > 16 {
930 roi.height = 16;
931 }
932
933 if roi.width > 10 || roi.height > 10 {
934 center[0] = 199;
935 }
936
937 let config = ((roi.height - 1) << 4 | (roi.width - 1)) as u8;
938
939 self.write_bytes(Register::ROI_CONFIG__MODE_ROI_CENTRE_SPAD, ¢er)?;
940 self.write_bytes(
941 Register::ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE,
942 &[config],
943 )?;
944
945 Ok(())
946 }
947
948 pub fn get_roi(&mut self) -> Result<ROI, Error<E>> {
953 let mut config = [0];
954
955 self.read_bytes(
956 Register::ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE,
957 &mut config,
958 )?;
959
960 Ok(ROI {
961 width: ((config[0] & 0x0F) + 1) as u16,
962 height: (((config[0] >> 4) & 0x0F) + 1) as u16,
963 })
964 }
965
966 pub fn set_roi_center(&mut self, center: ROICenter) -> Result<(), Error<E>> {
974 self.write_bytes(Register::ROI_CONFIG__USER_ROI_CENTRE_SPAD, &[center.spad])
975 }
976
977 pub fn get_roi_center(&mut self) -> Result<ROICenter, Error<E>> {
979 let mut center = [0];
980
981 self.read_bytes(Register::ROI_CONFIG__MODE_ROI_CENTRE_SPAD, &mut center)?;
982
983 Ok(ROICenter { spad: center[0] })
984 }
985
986 pub fn set_signal_threshold(&mut self, threshold: u16) -> Result<(), Error<E>> {
992 self.write_bytes(
993 Register::RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS,
994 &(threshold >> 3).to_be_bytes(),
995 )
996 }
997
998 pub fn get_signal_threshold(&mut self) -> Result<u16, Error<E>> {
1000 let mut threshold = [0, 0];
1001
1002 self.read_bytes(
1003 Register::RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS,
1004 &mut threshold,
1005 )?;
1006
1007 Ok(u16::from_be_bytes(threshold) << 3)
1008 }
1009
1010 pub fn set_sigma_threshold(&mut self, threshold: u16) -> Result<(), Error<E>> {
1016 if threshold > (0xFFFF >> 2) {
1017 return Err(Error::InvalidSigmaThreshold);
1018 }
1019
1020 self.write_bytes(
1021 Register::RANGE_CONFIG__SIGMA_THRESH,
1022 &(threshold << 2).to_be_bytes(),
1023 )
1024 }
1025
1026 pub fn get_sigma_threshold(&mut self) -> Result<u16, Error<E>> {
1028 let mut threshold = [0, 0];
1029
1030 self.read_bytes(Register::RANGE_CONFIG__SIGMA_THRESH, &mut threshold)?;
1031
1032 Ok(u16::from_be_bytes(threshold) >> 2)
1033 }
1034
1035 pub fn calibrate_temperature(&mut self) -> Result<(), Error<E>> {
1039 self.write_bytes(
1040 Register::VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND,
1041 &0x81u16.to_be_bytes(),
1042 )?;
1043 self.write_bytes(Register::VHV_CONFIG__INIT, &0x92u16.to_be_bytes())?;
1044
1045 self.start_ranging()?;
1046
1047 while !self.is_data_ready()? {}
1048
1049 self.clear_interrupt()?;
1050 self.stop_ranging()?;
1051
1052 self.write_bytes(
1053 Register::VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND,
1054 &0x09u16.to_be_bytes(),
1055 )?;
1056 self.write_bytes(Register::VHV_CONFIG__INIT, &0u16.to_be_bytes())?;
1057
1058 Ok(())
1059 }
1060
1061 pub fn calibrate_offset(&mut self, target_distance_mm: u16) -> Result<i16, Error<E>> {
1069 self.write_bytes(
1070 Register::ALGO__PART_TO_PART_RANGE_OFFSET_MM,
1071 &0u16.to_be_bytes(),
1072 )?;
1073 self.write_bytes(Register::MM_CONFIG__INNER_OFFSET_MM, &0u16.to_be_bytes())?;
1074 self.write_bytes(Register::MM_CONFIG__OUTER_OFFSET_MM, &0u16.to_be_bytes())?;
1075
1076 self.start_ranging()?;
1077
1078 let mut average_distance = 0;
1079
1080 let measurement_count = 50;
1081
1082 for _ in 0..measurement_count {
1083 while self.is_data_ready()? {}
1084
1085 average_distance += self.get_distance()?;
1086 self.clear_interrupt()?;
1087 }
1088
1089 self.stop_ranging()?;
1090
1091 average_distance /= measurement_count;
1092
1093 let offset = target_distance_mm as i16 - average_distance as i16;
1094
1095 self.set_offset(offset)?;
1096
1097 Ok(offset)
1098 }
1099
1100 pub fn calibrate_cross_talk(&mut self, target_distance_mm: u16) -> Result<u16, Error<E>> {
1108 self.write_bytes(
1109 Register::ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS,
1110 &0u16.to_be_bytes(),
1111 )?;
1112
1113 self.start_ranging()?;
1114
1115 let mut average_distance = 0;
1116 let mut average_spad_cnt = 0;
1117 let mut average_signal_rate = 0;
1118
1119 let measurement_count = 50;
1120
1121 for _ in 0..measurement_count {
1122 while self.is_data_ready()? {}
1123
1124 average_distance += self.get_distance()?;
1125 average_signal_rate += self.get_signal_rate()?;
1126 self.clear_interrupt()?;
1127 average_spad_cnt += self.get_spad_count()?;
1128 }
1129
1130 self.stop_ranging()?;
1131
1132 average_distance /= measurement_count;
1133 average_spad_cnt /= measurement_count;
1134 average_signal_rate /= measurement_count;
1135
1136 let mut calibrate_val = 512
1137 * (average_signal_rate as u32
1138 * (1 - (average_distance as u32 / target_distance_mm as u32)))
1139 / average_spad_cnt as u32;
1140
1141 if calibrate_val > 0xFFFF {
1142 calibrate_val = 0xFFFF;
1143 }
1144
1145 let config = ((calibrate_val as u16).saturating_mul(1000) >> 9).to_be_bytes();
1146
1147 self.write_bytes(
1148 Register::ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS,
1149 &config,
1150 )?;
1151
1152 Ok(0)
1153 }
1154}
1155
1156#[cfg(test)]
1157mod tests {
1158
1159 extern crate std;
1160
1161 use crate::{RangeStatus, Register, SWVersion, DEFAULT_ADDRESS, VL53L1X};
1162
1163 use embedded_hal_mock::{
1164 i2c::{Mock as I2cMock, Transaction as I2cTransaction},
1165 MockError,
1166 };
1167 use std::{io::ErrorKind, vec};
1168
1169 type VL53L1XMock = VL53L1X<I2cMock>;
1170
1171 #[test]
1172 fn sw_version() {
1173 assert!(
1174 VL53L1XMock::sw_version()
1175 == SWVersion {
1176 major: 3,
1177 minor: 5,
1178 build: 1,
1179 revision: 0
1180 }
1181 );
1182 }
1183
1184 #[test]
1185 fn set_address() {
1186 let new_address = 0x55;
1187
1188 let i2c_adr_cmd = (Register::I2C_SLAVE__DEVICE_ADDRESS as u16).to_be_bytes();
1189 let clr_irq_cmd = (Register::SYSTEM__INTERRUPT_CLEAR as u16).to_be_bytes();
1190
1191 let expectations = [
1192 I2cTransaction::write(0x29, vec![i2c_adr_cmd[0], i2c_adr_cmd[1], new_address]),
1193 I2cTransaction::write(0x55, vec![clr_irq_cmd[0], clr_irq_cmd[1], 0x01]),
1194 ];
1195
1196 let i2c = I2cMock::new(&expectations);
1197
1198 let mut sensor = VL53L1XMock::new(i2c, DEFAULT_ADDRESS);
1199
1200 assert!(sensor.set_address(new_address).is_ok());
1201 assert!(sensor.clear_interrupt().is_ok());
1202 }
1203
1204 #[test]
1205 fn set_address_driver_failure() {
1206 let new_address = 0x55;
1207
1208 let i2c_adr_cmd = (Register::I2C_SLAVE__DEVICE_ADDRESS as u16).to_be_bytes();
1209 let clr_irq_cmd = (Register::SYSTEM__INTERRUPT_CLEAR as u16).to_be_bytes();
1210
1211 let expectations = [
1212 I2cTransaction::write(0x29, vec![i2c_adr_cmd[0], i2c_adr_cmd[1], new_address])
1213 .with_error(MockError::Io(ErrorKind::NotFound)),
1214 I2cTransaction::write(0x29, vec![clr_irq_cmd[0], clr_irq_cmd[1], 0x01]),
1215 ];
1216
1217 let i2c = I2cMock::new(&expectations);
1218
1219 let mut sensor = VL53L1XMock::new(i2c, DEFAULT_ADDRESS);
1220
1221 assert!(sensor.set_address(new_address).is_err());
1222 assert!(sensor.clear_interrupt().is_ok());
1223 }
1224
1225 #[test]
1226 fn write_error_propegate() {
1227 let clr_irq_cmd = (Register::SYSTEM__INTERRUPT_CLEAR as u16).to_be_bytes();
1228
1229 let expectations =
1230 [
1231 I2cTransaction::write(0x29, vec![clr_irq_cmd[0], clr_irq_cmd[1], 0x01])
1232 .with_error(MockError::Io(ErrorKind::NotFound)),
1233 ];
1234
1235 let i2c = I2cMock::new(&expectations);
1236
1237 let mut sensor = VL53L1XMock::new(i2c, DEFAULT_ADDRESS);
1238
1239 let res = sensor.clear_interrupt();
1240
1241 assert!(res.is_err());
1242
1243 match res.unwrap_err() {
1244 crate::Error::CommunicationError(e) => {
1245 assert!(e == MockError::Io(ErrorKind::NotFound))
1246 }
1247 _ => panic!("Invalid error returned"),
1248 }
1249 }
1250
1251 #[test]
1252 fn write_read_error_propegate() {
1253 let irq_dir_cmd = (Register::GPIO_HV_MUX__CTRL as u16).to_be_bytes();
1254
1255 let expectations =
1256 [
1257 I2cTransaction::write_read(0x29, vec![irq_dir_cmd[0], irq_dir_cmd[1]], vec![0])
1258 .with_error(MockError::Io(ErrorKind::NotFound)),
1259 ];
1260
1261 let i2c = I2cMock::new(&expectations);
1262
1263 let mut sensor = VL53L1XMock::new(i2c, DEFAULT_ADDRESS);
1264
1265 let res = sensor.is_data_ready();
1266
1267 assert!(res.is_err());
1268
1269 match res.unwrap_err() {
1270 crate::Error::CommunicationError(e) => {
1271 assert!(e == MockError::Io(ErrorKind::NotFound))
1272 }
1273 _ => panic!("Invalid error returned"),
1274 }
1275 }
1276
1277 #[test]
1278 fn ambient_saturation() {
1279 let ambient_reg = (Register::RESULT__AMBIENT_COUNT_RATE_MCPS_SD0 as u16).to_be_bytes();
1280
1281 let expectations = [I2cTransaction::write_read(
1282 0x29,
1283 vec![ambient_reg[0], ambient_reg[1]],
1284 vec![0xFF, 0xFF],
1285 )];
1286
1287 let i2c = I2cMock::new(&expectations);
1288
1289 let mut sensor = VL53L1XMock::new(i2c, DEFAULT_ADDRESS);
1290
1291 assert!(sensor.get_ambient_rate().unwrap() == 0xFFFF);
1292 }
1293
1294 #[test]
1295 fn signal_saturation() {
1296 let signal_reg = (Register::RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0
1297 as u16)
1298 .to_be_bytes();
1299
1300 let expectations = [I2cTransaction::write_read(
1301 0x29,
1302 vec![signal_reg[0], signal_reg[1]],
1303 vec![0xFF, 0xFF],
1304 )];
1305
1306 let i2c = I2cMock::new(&expectations);
1307
1308 let mut sensor = VL53L1XMock::new(i2c, DEFAULT_ADDRESS);
1309
1310 assert!(sensor.get_signal_rate().unwrap() == 0xFFFF);
1311 }
1312
1313 #[test]
1314 fn result_saturation() {
1315 let status_reg = (Register::RESULT__RANGE_STATUS as u16).to_be_bytes();
1316
1317 let expectations = [I2cTransaction::write_read(
1318 0x29,
1319 vec![status_reg[0], status_reg[1]],
1320 vec![
1321 0, 0, 0, 0, 0, 0, 0, 0xFF, 0xFF, 0, 0, 0, 0, 0, 0, 0xFF, 0xFF,
1322 ],
1323 )];
1324
1325 let i2c = I2cMock::new(&expectations);
1326
1327 let mut sensor = VL53L1XMock::new(i2c, DEFAULT_ADDRESS);
1328
1329 let measurement = sensor.get_result().unwrap();
1330
1331 assert!(measurement.sig_per_spad == 0xFFFF);
1332 assert!(measurement.ambient == 0xFFFF);
1333 assert!(measurement.status == RangeStatus::None);
1334 assert!(measurement.spad_count == 0);
1335 assert!(measurement.distance_mm == 0);
1336 }
1337}