tof driver start (not much out)

controller/src/main.rs

@@ -6,10 +6,12 @@
 #![allow(async_fn_in_trait)]
 
 use core::fmt::Formatter;
+use core::panic::PanicInfo;
 use core::str::from_utf8;
 
 use bt_hci::cmd::info;
 use cyw43_pio::PioSpi;
+use embassy_rp::i2c::{Async, I2c};
 use log::*;
 //use embassy_rp::i2c::InterruptHandler;
 use embassy_executor::Spawner;
@@ -26,7 +28,7 @@ use embedded_io_async::Write;
 use rand::RngCore;
 use reqwless::response;
 use static_cell::StaticCell;
-use {defmt_rtt as _, panic_probe as _};
+use {defmt_rtt as _, };
 
 bind_interrupts!(struct Irqs {
     PIO0_IRQ_0 => InterruptHandler<PIO0>;
@@ -49,6 +51,12 @@ async fn net_task(mut runner: embassy_net::Runner<'static, cyw43::NetDriver<'sta
     runner.run().await
 }
 
+#[panic_handler]
+fn panic( info: &PanicInfo) -> ! {
+    error!("{}", info);
+    loop { }
+}
+
 #[embassy_executor::main]
 async fn main(spawner: Spawner) {
     info!("Hello World!");
@@ -116,6 +124,127 @@ async fn main(spawner: Spawner) {
 
     //embassy_time::Timer::after_millis(7000).await;
 
+    async fn write_to_device<'a, T,const N: usize>(bus: &mut embassy_rp::i2c::I2c<'a, T,Async>, addr: u16, data: [[u8;2];N])
+    where T: embassy_rp::i2c::Instance { 
+        for transaction in data {
+            let _ = bus.write_async(addr, transaction).await;
+        }
+    }
+
+    let id = 0x29;
+    Timer::after_millis(50).await; // sensor boot
+    write_to_device(&mut bus, id, [
+        [0x88, 0x00],
+        [0x80, 0x01],
+        [0xFF, 0x01],
+        [0x00, 0x00],
+    ]).await;
+    let mut stop: [u8;1] = [0];
+    let _ = bus.write_read_async(id, [0x91], &mut stop);
+    write_to_device(&mut bus, id, [
+        [0x00, 0x01],
+        [0xFF, 0x00],
+        [0x80, 0x00],
+    ]).await;
+    // TODO: VL53L0X.cpp L86-138
+    write_to_device(&mut bus, id, [
+        [0xFF, 0x01],
+        [0x00, 0x00],
+        [0xFF, 0x00],
+        [0x09, 0x00],
+        [0x10, 0x00],
+        [0x11, 0x00],
+        [0x24, 0x01],
+        [0x25, 0xFF],
+        [0x75, 0x00],
+        [0xFF, 0x01],
+        [0x4E, 0x2C],
+        [0x48, 0x00],
+        [0x30, 0x20],
+        [0xFF, 0x00],
+        [0x30, 0x09],
+        [0x54, 0x00],
+        [0x31, 0x04],
+        [0x32, 0x03],
+        [0x40, 0x83],
+        [0x46, 0x25],
+        [0x60, 0x00],
+        [0x27, 0x00],
+        [0x50, 0x06],
+        [0x51, 0x00],
+        [0x52, 0x96],
+        [0x56, 0x08],
+        [0x57, 0x30],
+        [0x61, 0x00],
+        [0x62, 0x00],
+        [0x64, 0x00],
+        [0x65, 0x00],
+        [0x66, 0xA0],
+        [0xFF, 0x01],
+        [0x22, 0x32],
+        [0x47, 0x14],
+        [0x49, 0xFF],
+        [0x4A, 0x00],
+        [0xFF, 0x00],
+        [0x7A, 0x0A],
+        [0x7B, 0x00],
+        [0x78, 0x21],
+        [0xFF, 0x01],
+        [0x23, 0x34],
+        [0x42, 0x00],
+        [0x44, 0xFF],
+        [0x45, 0x26],
+        [0x46, 0x05],
+        [0x40, 0x40],
+        [0x0E, 0x06],
+        [0x20, 0x1A],
+        [0x43, 0x40],
+        [0xFF, 0x00],
+        [0x34, 0x03],
+        [0x35, 0x44],
+        [0xFF, 0x01],
+        [0x31, 0x04],
+        [0x4B, 0x09],
+        [0x4C, 0x05],
+        [0x4D, 0x04],
+        [0xFF, 0x00],
+        [0x44, 0x00],
+        [0x45, 0x20],
+        [0x47, 0x08],
+        [0x48, 0x28],
+        [0x67, 0x00],
+        [0x70, 0x04],
+        [0x71, 0x01],
+        [0x72, 0xFE],
+        [0x76, 0x00],
+        [0x77, 0x00],
+        [0xFF, 0x01],
+        [0x0D, 0x01],
+        [0xFF, 0x00],
+        [0x80, 0x01],
+        [0x01, 0xF8],
+        [0xFF, 0x01],
+        [0x8E, 0x01],
+        [0x00, 0x01],
+        [0xFF, 0x00],
+        [0x80, 0x00],
+    ]).await;
+    //TODO VL53L0X.cpp L 236-280
+
+
+    // start continuous mode
+    write_to_device(&mut bus, id, [
+        [0x80, 0x01],
+        [0xFF, 0x01],
+        [0x00, 0x00],
+        [0x91, stop[0]],
+        [0x00, 0x01],
+        [0xFF, 0x00],
+        [0x80, 0x00],
+        //[0x04, 0x00], // measurement delay
+        [0x00, 0x02], // back to back shots
+    ]).await;
+
     //let _ = bus.write_async(0x88u16, [0x00]).await;
     //let _ = bus.write_async(0x80u16, [0x01]).await;
     //let _ = bus.write_async(0xFFu16, [0x01]).await;
@@ -124,6 +253,13 @@ async fn main(spawner: Spawner) {
     //let _ = bus.write_async(0xFFu16, [0x00]).await;
     //let _ = bus.write_async(0x80u16, [0x00]).await;
     
+    //loop {
+    //    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;
+    //    info!("{:?}",output);
+    //    Timer::after_millis(20).await;
+    //}
 
     loop {
         let mut socket = TcpSocket::new(stack, &mut rx_buffer, &mut tx_buffer);
@@ -156,7 +292,7 @@ async fn main(spawner: Spawner) {
             info!("rxd {}", from_utf8(&buf[..n]).unwrap());
             Timer::after_millis(30).await;
 
-            let mut segs = buf[..n].split(|c| *c == ' ' as u8);
+            let mut segs = buf[..n].trim_ascii().split(|c| *c == ' ' as u8);
 
             match char::from_u32(segs.next().unwrap()[0] as u32).unwrap() {
                 'W' => {