From 8a10953cc43777e95e467235ec2616103b0b1dc9 Mon Sep 17 00:00:00 2001 From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com> Date: Sat, 28 Sep 2024 17:55:15 -0400 Subject: [PATCH] tof init (not successful) --- controller/src/main.rs | 153 ++++++++++++++++++++++++++++++++++++++++- 1 file changed, 151 insertions(+), 2 deletions(-) diff --git a/controller/src/main.rs b/controller/src/main.rs index 9a866e7..0e1f578 100644 --- a/controller/src/main.rs +++ b/controller/src/main.rs @@ -5,6 +5,9 @@ #![no_main] #![allow(async_fn_in_trait)] +mod vl53l0; + +use core::array; use core::fmt::Formatter; use core::panic::PanicInfo; use core::str::from_utf8; @@ -28,7 +31,8 @@ use embedded_io_async::Write; use rand::RngCore; use reqwless::response; use static_cell::StaticCell; -use {defmt_rtt as _, }; +use defmt_rtt as _; +use vl53l0::RegAddr::*; bind_interrupts!(struct Irqs { PIO0_IRQ_0 => InterruptHandler; @@ -131,6 +135,19 @@ async fn main(spawner: Spawner) { } } + async fn write_flag<'a, T>(bus: &mut embassy_rp::i2c::I2c<'a, T,Async>, addr: u16, reg: u8, bit: u8, value: bool) + where T: embassy_rp::i2c::Instance { + let mut initial: [u8;1] = [0]; + let _ = bus.write_read_async(addr, [reg], &mut initial).await; + let mask = 1 << bit; + if value { + initial[0] |= mask; + } else { + initial[0] &= !mask; + } + let _ = bus.write_async(addr, [reg, initial[0]]).await; + } + let id = 0x29; Timer::after_millis(50).await; // sensor boot write_to_device(&mut bus, id, [ @@ -146,7 +163,92 @@ async fn main(spawner: Spawner) { [0xFF, 0x00], [0x80, 0x00], ]).await; - // TODO: VL53L0X.cpp L86-138 + + // disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks + write_flag(&mut bus, id, 0x60, 1, true).await; + write_flag(&mut bus, id, 0x60, 4, true).await; + + let mega_counts_per_second = 0.25; + let mega_counts_per_second: u16 = (mega_counts_per_second * (1<<7) as f64) as u16; + let _ = bus.write_async(id, [FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT as u8, + (mega_counts_per_second >> 8) as u8, + (mega_counts_per_second & 255) as u8 + ]).await; + + // get spad info + let _ = bus.write_async(id, [SYSTEM_SEQUENCE_CONFIG as u8, 0xFF]).await; + + write_to_device(&mut bus, id , [ + [0x80, 0x01], + [0xff, 0x01], + [0x00, 0x00], + [0xff, 0x06], + ]).await; + write_flag(&mut bus, id, 0x83, 3, true).await; + write_to_device(&mut bus, id , [ + [0xff, 0x07], + [0x81, 0x01], + [0x80, 0x01], + [0x94, 0x6b], + [0x83, 0x00], + ]).await; + debug!("starting spad wait"); + loop { + let mut wait: [u8;1] = [0]; + let _ = bus.write_read_async(id, [0x83], &mut wait).await; + if wait[0] != 0 { + break; + } + Timer::after_micros(5).await; + } + debug!("ended spad wait"); + + let _ = bus.write_async(id, [0x83, 0x01]).await; + + let mut value: [u8;1] = [0]; + let _ = bus.write_read_async(id, [0x92], &mut value).await; + write_to_device(&mut bus, id, [ + [0x81, 0x00], + [0xff, 0x06], + ]).await; + write_flag(&mut bus, id, 0x83, 3, false).await; + write_to_device(&mut bus, id, [ + [0xff, 0x01], + [0x00, 0x01], + [0xff, 0x00], + [0x80, 0x00], + ]).await; + let count = value[0] & 0x7f; + let is_aperture = value[0] & 0b10000000; + let is_aperture = is_aperture != 0; + // TODO: vl53l0x.py post line 200 + + let mut spad_map: [u8;6]=[0;6]; + let _ = bus.write_read_async(id, [GLOBAL_CONFIG_SPAD_ENABLES_REF_0 as u8], &mut spad_map); + + write_to_device(&mut bus, id, [ + [0xff, 0x01], + [DYNAMIC_SPAD_REF_EN_START_OFFSET as u8, 0x00], + [DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD as u8, 0x2c], + [0xff, 0x00], + [DYNAMIC_SPAD_REF_EN_START_OFFSET as u8, 0xb4], + ]).await; + + let mut spads_enabled = 0; + for i in 0..48 { + if i < 12 && is_aperture || spads_enabled >= count { + spad_map[i/8] &= !(1<< (i>>2)); + } else if (spad_map[i/8] & (1<< (i>>2))) != 0 { + spads_enabled += 1; + } + } + + let mut spad_write: [u8;7] = [0;7]; + spad_write[0] = GLOBAL_CONFIG_SPAD_ENABLES_REF_0 as u8; + spad_write[1..].clone_from_slice(&spad_map); + let _ = bus.write_async(id, spad_write).await; + + write_to_device(&mut bus, id, [ [0xFF, 0x01], [0x00, 0x00], @@ -229,6 +331,33 @@ async fn main(spawner: Spawner) { [0xFF, 0x00], [0x80, 0x00], ]).await; + //write_flag(&mut bus, id, GPIO_HV_MUX_ACTIVE_HIGH as u8, 4, false).await; + //let _ = bus.write_async(id, [SYSTEM_SEQUENCE_CONFIG as u8, 0x01]).await; + //calibrate(&mut bus, id, 0x40).await; + //let _ = bus.write_async(id, [SYSTEM_SEQUENCE_CONFIG as u8, 0x02]).await; + //calibrate(&mut bus, id, 0x00).await; + //let _ = bus.write_async(id, [SYSTEM_SEQUENCE_CONFIG as u8, 0xe8]).await; + + async fn calibrate<'a, T>(bus: &mut embassy_rp::i2c::I2c<'a, T,Async>, addr: u16, data: u8) + where T: embassy_rp::i2c::Instance { + let _ = bus.write_async(addr, [SYSRANGE_START as u8 ,data | 0x01]).await; + debug!("started calib wait"); + loop { + let mut wait: [u8;1] = [0]; + let _ = bus.write_read_async(addr, [0x13], &mut wait).await; + if wait[0] & 0x07 != 0 { + break; + } + Timer::after_micros(5).await; + } + debug!("ended calib wait"); + write_to_device(bus, addr, [ + [SYSTEM_INTERRUPT_CLEAR as u8, 0x01], + [SYSRANGE_START as u8, 0x00], + ]).await; + } + + //TODO VL53L0X.cpp L 236-280 @@ -254,6 +383,26 @@ async fn main(spawner: Spawner) { //let _ = bus.write_async(0x80u16, [0x00]).await; //loop { + // debug!("starting wait part 1"); + // loop { + // let mut wait: [u8;1] = [0]; + // let _ = bus.write_read_async(id, [SYSRANGE_START as u8], &mut wait).await; + // debug!("{wait:?}"); + // if wait[0] & 0x01 != 0 { + // break; + // } + // Timer::after_micros(5).await; + // } + // debug!("starting wait part 2"); + // loop { + // let mut wait: [u8;1] = [0]; + // let _ = bus.write_read_async(id, [RESULT_INTERRUPT_STATUS as u8], &mut wait).await; + // if wait[0] & 0x07 != 0 { + // break; + // } + // Timer::after_micros(5).await; + // } + // debug!("ending wait part 2"); // let mut output: [u8; 2] = [0;2]; // let _ = bus.write_read_async(id, [0x14], &mut output).await; // let _ = bus.write_async(id, [0x0B, 0x01]).await;