diff --git a/controller/src/main.rs b/controller/src/main.rs index 61e2f86..e066dfb 100644 --- a/controller/src/main.rs +++ b/controller/src/main.rs @@ -178,292 +178,6 @@ async fn main(spawner: Spawner) { let mut tx_buffer = [0; 4096]; let mut buf = [0; 4096]; - //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; - // } - //} - - //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, [ - // [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; - // - //// 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], - // [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], - - // [SYSTEM_INTERRUPT_CONFIG_GPIO as u8, 0x04], - //]).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 - - - //// 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; - ////let _ = bus.write_async(0x00u16, [0x00]).await; - ////let _ = bus.write_async(0x00u16, [0x01]).await; - ////let _ = bus.write_async(0xFFu16, [0x00]).await; - ////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; - //// info!("out: {:?}",output); - //// Timer::after_millis(20).await; - ////} - loop { let mut socket = TcpSocket::new(stack, &mut rx_buffer, &mut tx_buffer); socket.set_timeout(Some(Duration::from_secs(3)));