493 lines
16 KiB
Rust
493 lines
16 KiB
Rust
//! This example uses the RP Pico W board Wifi chip (cyw43).
|
|
//! Creates an Access point Wifi network and creates a TCP endpoint on port 1234.
|
|
|
|
#![no_std]
|
|
#![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;
|
|
|
|
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;
|
|
use embassy_net::tcp::TcpSocket;
|
|
use embassy_net::{Config, StackResources};
|
|
use embassy_rp::bind_interrupts;
|
|
use embassy_rp::clocks::RoscRng;
|
|
use embassy_rp::gpio::{Level, Output};
|
|
use embassy_rp::peripherals::{DMA_CH0, PIO0, USB};
|
|
use embassy_rp::pio::{InterruptHandler, Pio};
|
|
use embassy_rp::usb::Driver;
|
|
use embassy_time::{Duration, Timer};
|
|
use embedded_io_async::Write;
|
|
use rand::RngCore;
|
|
use reqwless::response;
|
|
use static_cell::StaticCell;
|
|
use defmt_rtt as _;
|
|
use vl53l0::RegAddr::*;
|
|
|
|
bind_interrupts!(struct Irqs {
|
|
PIO0_IRQ_0 => InterruptHandler<PIO0>;
|
|
I2C1_IRQ => embassy_rp::i2c::InterruptHandler<embassy_rp::peripherals::I2C1>;
|
|
USBCTRL_IRQ => embassy_rp::usb::InterruptHandler<USB>;
|
|
});
|
|
|
|
#[embassy_executor::task]
|
|
async fn logger_task(driver: Driver<'static, USB>) {
|
|
embassy_usb_logger::run!(1024, log::LevelFilter::Debug, driver);
|
|
}
|
|
|
|
#[embassy_executor::task]
|
|
async fn cyw43_task(runner: cyw43::Runner<'static, Output<'static>, PioSpi<'static, PIO0, 0, DMA_CH0>>) -> ! {
|
|
runner.run().await
|
|
}
|
|
|
|
#[embassy_executor::task]
|
|
async fn net_task(mut runner: embassy_net::Runner<'static, cyw43::NetDriver<'static>>) -> ! {
|
|
runner.run().await
|
|
}
|
|
|
|
#[panic_handler]
|
|
fn panic( info: &PanicInfo) -> ! {
|
|
error!("{}", info);
|
|
loop { }
|
|
}
|
|
|
|
#[embassy_executor::main]
|
|
async fn main(spawner: Spawner) {
|
|
info!("Hello World!");
|
|
|
|
let p = embassy_rp::init(Default::default());
|
|
let mut rng = RoscRng;
|
|
|
|
let driver = Driver::new(p.USB, Irqs);
|
|
spawner.spawn(logger_task(driver)).unwrap();
|
|
|
|
let sda = p.PIN_26;
|
|
let scl = p.PIN_27;
|
|
let config = embassy_rp::i2c::Config::default();
|
|
let mut bus = embassy_rp::i2c::I2c::new_async(p.I2C1, scl, sda, Irqs, config);
|
|
|
|
let fw = include_bytes!("../../cyw43-firmware/43439A0.bin");
|
|
let clm = include_bytes!("../../cyw43-firmware/43439A0_clm.bin");
|
|
|
|
// To make flashing faster for development, you may want to flash the firmwares independently
|
|
// at hardcoded addresses, instead of baking them into the program with `include_bytes!`:
|
|
// probe-rs download 43439A0.bin --binary-format bin --chip RP2040 --base-address 0x10100000
|
|
// probe-rs download 43439A0_clm.bin --binary-format bin --chip RP2040 --base-address 0x10140000
|
|
//let fw = unsafe { core::slice::from_raw_parts(0x10100000 as *const u8, 230321) };
|
|
//let clm = unsafe { core::slice::from_raw_parts(0x10140000 as *const u8, 4752) };
|
|
|
|
let pwr = Output::new(p.PIN_23, Level::Low);
|
|
let cs = Output::new(p.PIN_25, Level::High);
|
|
let mut pio = Pio::new(p.PIO0, Irqs);
|
|
let spi = PioSpi::new(&mut pio.common, pio.sm0, pio.irq0, cs, p.PIN_24, p.PIN_29, p.DMA_CH0);
|
|
|
|
static STATE: StaticCell<cyw43::State> = StaticCell::new();
|
|
let state = STATE.init(cyw43::State::new());
|
|
let (net_device, mut control, runner) = cyw43::new(state, pwr, spi, fw).await;
|
|
defmt::unwrap!(spawner.spawn(cyw43_task(runner)));
|
|
|
|
control.init(clm).await;
|
|
control
|
|
.set_power_management(cyw43::PowerManagementMode::PowerSave)
|
|
.await;
|
|
|
|
// Use a link-local address for communication without DHCP server
|
|
let config = Config::ipv4_static(embassy_net::StaticConfigV4 {
|
|
address: embassy_net::Ipv4Cidr::new(embassy_net::Ipv4Address::new(169, 254, 1, 1), 16),
|
|
dns_servers: heapless::Vec::new(),
|
|
gateway: None,
|
|
});
|
|
|
|
// Generate random seed
|
|
let seed = rng.next_u64();
|
|
|
|
// Init network stack
|
|
static RESOURCES: StaticCell<StackResources<3>> = StaticCell::new();
|
|
let (stack, runner) = embassy_net::new(net_device, config, RESOURCES.init(StackResources::new()), seed);
|
|
|
|
defmt::unwrap!(spawner.spawn(net_task(runner)));
|
|
|
|
//control.start_ap_open("cyw43", 5).await;
|
|
control.start_ap_wpa2("cyw43", "password", 5).await;
|
|
|
|
// And now we can use it!
|
|
|
|
let mut rx_buffer = [0; 4096];
|
|
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],
|
|
]).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!("{:?}",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(10)));
|
|
|
|
control.gpio_set(0, false).await;
|
|
info!("Listening on TCP:1234...");
|
|
if let Err(e) = socket.accept(1234).await {
|
|
warn!("accept error: {:?}", e);
|
|
continue;
|
|
}
|
|
|
|
info!("Received connection from {:?}", socket.remote_endpoint());
|
|
control.gpio_set(0, true).await;
|
|
|
|
loop {
|
|
let mut n = match socket.read(&mut buf).await {
|
|
Ok(0) => {
|
|
warn!("read EOF");
|
|
break;
|
|
}
|
|
Ok(n) => n,
|
|
Err(e) => {
|
|
warn!("read error: {:?}", e);
|
|
break;
|
|
}
|
|
};
|
|
|
|
|
|
info!("rxd {}", from_utf8(&buf[..n]).unwrap());
|
|
Timer::after_millis(30).await;
|
|
|
|
let mut segs = buf[..n].trim_ascii().split(|c| *c == ' ' as u8);
|
|
|
|
match char::from_u32(segs.next().unwrap()[0] as u32).unwrap() {
|
|
'W' => {
|
|
let addr: [u8;1] = hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
|
|
let reg: [u8;1] = hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
|
|
let data: [u8;1] = hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
|
|
info!("writing {:?}", reg);
|
|
Timer::after_millis(30).await;
|
|
|
|
bus.write_async(addr[0] as u16, [reg[0], data[0]]).await.unwrap();
|
|
|
|
buf[0] = b"O"[0];
|
|
buf[1] = b"K"[0];
|
|
n=2;
|
|
},
|
|
'R' => {
|
|
let addr: [u8;1] = hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
|
|
let reg: [u8;1] = hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
|
|
info!("writing {:?}, addr {addr:?}", reg);
|
|
|
|
let mut response: [u8;2] = [0;2];
|
|
|
|
let _ = bus.write_read_async(addr[0] as u16, reg, &mut response).await;
|
|
info!("recd {:?}", response);
|
|
info!("recd {:#02x}", response[0]);
|
|
let _ = hex::encode_to_slice(response, &mut buf);
|
|
n = 4;
|
|
|
|
|
|
},
|
|
_ => {}
|
|
|
|
}
|
|
|
|
//let mut response: [u8;2] = [0;2];
|
|
|
|
//let _ = bus.read_async(0xC0u16, &mut response).await;
|
|
//let _ = hex::encode_to_slice(response, &mut buf);
|
|
|
|
match socket.write_all(&buf[..n]).await {
|
|
Ok(()) => {}
|
|
Err(e) => {
|
|
warn!("write error: {:?}", e);
|
|
break;
|
|
}
|
|
};
|
|
}
|
|
}
|
|
}
|