#![warn(missing_docs)]
#![cfg_attr(not(feature = "std"), no_std)]
mod i2c;
pub mod wait;
use wait::WaitForMeasurement;
use core::fmt;
use embedded_hal_async::{delay::DelayNs, i2c::I2c};
use i2c::Device;
pub(crate) const DEFAULT_CONFIG_MSG: &[u8] = &[
0x00, 0x2d, 0x12, 0x00, 0x00, 0x11, 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, 0x14, 0x21, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00, 0xC8, 0x00, 0x00, 0x38, 0xFF, 0x01, 0x00, 0x08, 0x00, 0x00, 0x01, 0xCC, 0x07, 0x01, 0xF1, 0x05, 0x00, 0xA0, 0x00, 0x80, 0x08, 0x38, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x89, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x07, 0x05, 0x06, 0x06, 0x00, 0x00, 0x02, 0xC7, 0xFF, 0x9B, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, ];
#[derive(Debug, Clone, Copy)]
#[allow(non_camel_case_types)]
#[allow(missing_docs)]
#[cfg_attr(feature = "defmt-03", derive(defmt::Format))]
pub enum Register {
OSC_FREQ = 0x0006,
VHV_CONFIG_TIMEOUT_MACROP_LOOP_BOUND = 0x0008,
MYSTERY_1 = 0x000b,
MYSTERY_2 = 0x0024,
SYSTEM_START = 0x0087,
GPIO_HV_MUX_CTRL = 0x0030,
GPIO_TIO_HV_STATUS = 0x0031,
RANGE_CONFIG_A = 0x005e,
RANGE_CONFIG_B = 0x0061,
INTERMEASUREMENT_MS = 0x006c,
SYSTEM_INTERRUPT_CLEAR = 0x0086,
RESULT_RANGE_STATUS = 0x0089,
RESULT_NUM_SPADS = 0x008c,
RESULT_SIGNAL_RATE = 0x008e,
RESULT_AMBIENT_RATE = 0x0090,
RESULT_SIGMA = 0x0092,
RESULT_DISTANCE = 0x0096,
RESULT_OSC_CALIBRATE_VAL = 0x00de,
SYSTEM_STATUS = 0x00e5,
IDENTIFICATION_MODEL_ID = 0x010f,
}
impl Register {
pub const fn addr(&self) -> u16 {
*self as u16
}
pub const fn as_bytes(&self) -> [u8; 2] {
self.addr().to_be_bytes()
}
}
pub const PERIPHERAL_ADDR: u8 = 0x29;
#[derive(Debug, PartialEq, Eq, Clone, Copy)]
#[cfg_attr(feature = "defmt-03", derive(defmt::Format))]
#[repr(u8)]
pub enum Status {
Valid = 0,
SigmaAboveThreshold,
SigmaBelowThreshold,
DistanceBelowDetectionThreshold,
InvalidPhase,
HardwareFail,
NoWrapAroundCheck,
WrappedTargetPhaseMismatch,
ProcessingFail,
XTalkFail,
InterruptError,
MergedTarget,
SignalTooWeak,
Other = 255,
}
#[derive(Debug, PartialEq, Eq, Clone, Copy, PartialOrd, Ord)]
#[cfg_attr(feature = "defmt-03", derive(defmt::Format))]
pub enum Severity {
None,
Warning,
Error,
}
impl Status {
const fn from_rtn(rtn: u8) -> Self {
assert!(rtn < 24);
match rtn {
3 => Self::HardwareFail,
4 | 5 => Self::SigmaBelowThreshold,
6 => Self::SigmaAboveThreshold,
7 => Self::WrappedTargetPhaseMismatch,
8 => Self::DistanceBelowDetectionThreshold,
9 => Self::Valid,
12 => Self::XTalkFail,
13 => Self::InterruptError,
18 => Self::InterruptError,
19 => Self::NoWrapAroundCheck,
22 => Self::MergedTarget,
23 => Self::SignalTooWeak,
_ => Self::Other,
}
}
pub const fn severity(&self) -> Severity {
match self {
Status::Valid => Severity::None,
Status::SigmaAboveThreshold => Severity::Warning,
Status::SigmaBelowThreshold => Severity::Warning,
Status::DistanceBelowDetectionThreshold => Severity::Error,
Status::InvalidPhase => Severity::Error,
Status::HardwareFail => Severity::Error,
Status::NoWrapAroundCheck => Severity::Warning,
Status::WrappedTargetPhaseMismatch => Severity::Error,
Status::ProcessingFail => Severity::Error,
Status::XTalkFail => Severity::Error,
Status::InterruptError => Severity::Error,
Status::MergedTarget => Severity::Error,
Status::SignalTooWeak => Severity::Error,
Status::Other => Severity::Error,
}
}
}
#[derive(Debug, Clone, Copy)]
#[cfg_attr(feature = "defmt-03", derive(defmt::Format))]
pub struct Measurement {
pub status: Status,
pub distance: u16,
pub ambient_rate: u16,
pub signal_rate: u16,
pub spads_enabled: u16,
pub sigma: u16,
}
impl Measurement {
pub fn is_valid(&self) -> bool {
self.status == Status::Valid
}
}
pub struct Vl53l4cd<I2C, DELAY, WAIT> {
i2c: Device<I2C>,
delay: DELAY,
wait: WAIT,
}
impl<I2C: I2c, DELAY: DelayNs, WAIT: WaitForMeasurement<I2C, DELAY>> Vl53l4cd<I2C, DELAY, WAIT> {
pub const fn new(bus: I2C, delay: DELAY, wait: WAIT) -> Self {
Self::with_addr(bus, PERIPHERAL_ADDR, delay, wait)
}
pub const fn with_addr(bus: I2C, addr: u8, delay: DELAY, wait: WAIT) -> Self {
Self {
i2c: Device { addr, bus },
delay,
wait,
}
}
pub async fn init(&mut self) -> Result<(), Error<I2C::Error>> {
let id = self
.i2c
.read_word(Register::IDENTIFICATION_MODEL_ID)
.await?;
if id != 0xebaa {
#[cfg(feature = "defmt-03")]
defmt::error!("strange device id {:#06x}", id);
return Err(Error::InvalidArgument);
}
#[cfg(feature = "defmt-03")]
defmt::debug!("waiting for boot");
self.wait_for_boot().await?;
#[cfg(feature = "defmt-03")]
defmt::debug!("booted");
self.i2c.write(DEFAULT_CONFIG_MSG).await?;
self.start_ranging().await?;
self.stop_ranging().await?;
self.i2c
.write_byte(Register::VHV_CONFIG_TIMEOUT_MACROP_LOOP_BOUND, 0x09)
.await?;
self.i2c.write_byte(Register::MYSTERY_1, 0).await?;
self.i2c.write_word(Register::MYSTERY_2, 0x500).await?;
self.set_range_timing(50, 0).await?;
Ok(())
}
async fn wait_for_boot(&mut self) -> Result<(), Error<I2C::Error>> {
for _ in 0u16..1000 {
if self.i2c.read_byte(Register::SYSTEM_STATUS).await? == 0x3 {
return Ok(());
}
self.delay.delay_ms(1).await;
}
#[cfg(feature = "defmt-03")]
defmt::error!("timeout waiting for boot");
Err(Error::Timeout)
}
pub async fn set_range_timing(
&mut self,
timing_budget_ms: u32,
inter_measurement_ms: u32,
) -> Result<(), Error<I2C::Error>> {
assert!(
(10..=200).contains(&timing_budget_ms),
"timing budget must be in range [10, 200]"
);
let osc_freq = self.i2c.read_word(Register::OSC_FREQ).await?;
if osc_freq == 0 {
#[cfg(feature = "defmt-03")]
defmt::error!("oscillation frequency is zero");
return Err(Error::InvalidArgument);
}
let mut timing_budget_us = timing_budget_ms * 1000;
if inter_measurement_ms == 0 {
self.i2c
.write_dword(Register::INTERMEASUREMENT_MS, 0)
.await?;
timing_budget_us -= 2500;
} else {
assert!(
inter_measurement_ms <= timing_budget_ms,
"timing budget must be greater than or equal to inter-measurement"
);
let clock_pll = u32::from(
self.i2c
.read_word(Register::RESULT_OSC_CALIBRATE_VAL)
.await?
& 0x3ff,
);
let inter_measurement_fac = 1.055 * (inter_measurement_ms * clock_pll) as f32;
self.i2c
.write_dword(Register::INTERMEASUREMENT_MS, inter_measurement_fac as u32)
.await?;
timing_budget_us -= 4300;
timing_budget_us /= 2;
}
let (a, b) = range_config_values(timing_budget_us, osc_freq);
self.i2c.write_word(Register::RANGE_CONFIG_A, a).await?;
self.i2c.write_word(Register::RANGE_CONFIG_B, b).await?;
Ok(())
}
pub async fn measure(&mut self) -> Result<Measurement, Error<I2C::Error>> {
self.wait_for_measurement().await?;
#[cfg(feature = "defmt-03")]
defmt::debug!("measurement ready; reading");
let measurement = self.read_measurement().await?;
self.clear_interrupt().await?;
Ok(measurement)
}
pub async fn start_temperature_update(&mut self) -> Result<(), Error<I2C::Error>> {
self.i2c
.write_byte(Register::VHV_CONFIG_TIMEOUT_MACROP_LOOP_BOUND, 0x81)
.await?;
self.i2c.write_byte(Register::MYSTERY_1, 0x92).await?;
self.i2c.write_byte(Register::SYSTEM_START, 0x40).await?;
self.wait_for_measurement().await?;
self.clear_interrupt().await?;
self.stop_ranging().await?;
self.i2c
.write_byte(Register::VHV_CONFIG_TIMEOUT_MACROP_LOOP_BOUND, 0x09)
.await?;
self.i2c.write_byte(Register::MYSTERY_1, 0).await?;
Ok(())
}
#[inline]
pub async fn wait_for_measurement(&mut self) -> Result<(), Error<I2C::Error>> {
#[cfg(feature = "defmt-03")]
defmt::debug!("waiting for measurement");
self.wait
.wait_for_measurement(&mut self.i2c, &mut self.delay)
.await
}
#[inline]
pub async fn has_measurement(&mut self) -> Result<bool, Error<I2C::Error>> {
Ok(wait::has_measurement(&mut self.i2c).await?)
}
pub async fn read_measurement(&mut self) -> Result<Measurement, Error<I2C::Error>> {
let status = self.i2c.read_byte(Register::RESULT_RANGE_STATUS).await? & 0x1f;
Ok(Measurement {
status: Status::from_rtn(status),
distance: self.i2c.read_word(Register::RESULT_DISTANCE).await?,
spads_enabled: self.i2c.read_word(Register::RESULT_NUM_SPADS).await? / 256,
ambient_rate: self.i2c.read_word(Register::RESULT_AMBIENT_RATE).await? * 8,
signal_rate: self.i2c.read_word(Register::RESULT_SIGNAL_RATE).await? * 8,
sigma: self.i2c.read_word(Register::RESULT_SIGMA).await? / 4,
})
}
pub async fn clear_interrupt(&mut self) -> Result<(), Error<I2C::Error>> {
self.i2c
.write_byte(Register::SYSTEM_INTERRUPT_CLEAR, 0x01)
.await?;
Ok(())
}
pub async fn start_ranging(&mut self) -> Result<(), Error<I2C::Error>> {
if self.i2c.read_word(Register::INTERMEASUREMENT_MS).await? == 0 {
self.i2c.write_byte(Register::SYSTEM_START, 0x21).await?;
} else {
self.i2c.write_byte(Register::SYSTEM_START, 0x40).await?;
}
self.wait_for_measurement().await?;
self.clear_interrupt().await
}
pub async fn stop_ranging(&mut self) -> Result<(), Error<I2C::Error>> {
self.i2c.write_byte(Register::SYSTEM_START, 0x00).await?;
Ok(())
}
}
pub fn range_config_values(mut timing_budget_us: u32, osc_freq: u16) -> (u16, u16) {
let macro_period_us = (2304 * (0x40000000 / u32::from(osc_freq))) >> 6;
timing_budget_us <<= 12;
let f = |x: u32| {
let mut ms_byte = 0;
let tmp = macro_period_us * x;
let mut ls_byte = ((timing_budget_us + (tmp >> 7)) / (tmp >> 6)) - 1;
while (ls_byte & 0xffffff00) > 0 {
ls_byte >>= 1;
ms_byte += 1;
}
(ms_byte << 8) | (ls_byte & 0xff) as u16
};
(f(16), f(12))
}
#[derive(Debug)]
pub enum Error<E> {
I2c(E),
Gpio,
InvalidArgument,
Timeout,
}
impl<E> From<E> for Error<E> {
fn from(e: E) -> Self {
Self::I2c(e)
}
}
impl<E: embedded_hal_async::i2c::Error> fmt::Display for Error<E> {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Error::I2c(err) => write!(f, "i2c error: {}", err.kind()),
Error::Gpio => write!(f, "gpio error"),
Error::InvalidArgument => write!(f, "invalid argument"),
Error::Timeout => write!(f, "timeout"),
}
}
}
#[cfg(feature = "std")]
impl<E> std::error::Error for Error<E> where E: embedded_hal_async::i2c::Error {}