ank/cruisecontrol
1
Fork 0
Code Issues Activity

remove partial tof driver

Browse source
This commit is contained in:
Andy Killorin 2025-01-18 13:31:25 -05:00
parent 9f740d8435
commit 3b829b1b59
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
1 changed files with 0 additions and 286 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

286
controller/src/main.rs
View file

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