tof init (not successful)

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<PIO0>;
@@ -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;