rx netcode

copy pwm config from door winch
remove partial tof driver
2025-01-18 14:26:13 -05:00 · 2025-01-18 13:45:49 -05:00 · 2025-01-18 13:31:25 -05:00 · 2025-01-18 13:31:10 -05:00 · 2025-01-18 13:27:48 -05:00 · 2025-01-18 13:10:07 -05:00
6 changed files with 656 additions and 699 deletions
--- a/controller/Cargo.lock
+++ b/controller/Cargo.lock
--- a/controller/Cargo.toml
+++ b/controller/Cargo.toml
@ -6,16 +6,17 @@ license = "MIT OR Apache-2.0"


 [dependencies]
-embassy-executor = { version = "0.6.0", git="https://github.com/embassy-rs/embassy", features = ["task-arena-size-98304", "arch-cortex-m", "executor-thread", "executor-interrupt", "defmt", "integrated-timers"] }
-embassy-time = { version = "0.3.2", git="https://github.com/embassy-rs/embassy", features = ["defmt", "defmt-timestamp-uptime"] }
-embassy-rp = { version = "0.2.0", git="https://github.com/embassy-rs/embassy", features = ["defmt", "unstable-pac", "time-driver", "critical-section-impl", "rp2040"] }
-embassy-usb = { version = "0.3.0", git="https://github.com/embassy-rs/embassy", features = ["defmt"] }
-embassy-net = { version = "0.4.0", git="https://github.com/embassy-rs/embassy", features = ["defmt", "tcp", "udp", "raw", "dhcpv4", "medium-ethernet", "dns"] }
-embassy-net-wiznet = { version = "0.1.0", git="https://github.com/embassy-rs/embassy", features = ["defmt"] }
+embassy-executor = { version = "0.7.0", git="https://github.com/embassy-rs/embassy", features = ["task-arena-size-98304", "arch-cortex-m", "executor-thread", "executor-interrupt", "defmt"] }
+embassy-time = { version = "0.4.0", git="https://github.com/embassy-rs/embassy", features = ["defmt", "defmt-timestamp-uptime"] }
+embassy-rp = { version = "0.3.0", git="https://github.com/embassy-rs/embassy", features = ["defmt", "unstable-pac", "time-driver", "critical-section-impl", "rp2040"] }
+embassy-sync = { version = "0.6.2", git="https://github.com/embassy-rs/embassy" }
+embassy-usb = { version = "0.4.0", git="https://github.com/embassy-rs/embassy", features = ["defmt"] }
+embassy-net = { version = "0.6.0", git="https://github.com/embassy-rs/embassy", features = ["defmt", "tcp", "udp", "raw", "dhcpv4", "medium-ethernet", "dns"] }
+embassy-net-wiznet = { version = "0.2.0", git="https://github.com/embassy-rs/embassy", features = ["defmt"] }
 embassy-futures = { version = "0.1.0",  git="https://github.com/embassy-rs/embassy"}
-embassy-usb-logger = { version = "0.2.0",  git="https://github.com/embassy-rs/embassy" }
-cyw43 = { version = "0.2.0",   git="https://github.com/embassy-rs/embassy", features = ["defmt", "firmware-logs", "bluetooth"] }
-cyw43-pio = { version = "0.2.0",  git="https://github.com/embassy-rs/embassy", features = ["defmt"] }
+embassy-usb-logger = { version = "0.4.0",  git="https://github.com/embassy-rs/embassy" }
+cyw43 = { version = "0.3.0",   git="https://github.com/embassy-rs/embassy", features = ["defmt", "firmware-logs", "bluetooth"] }
+cyw43-pio = { version = "0.3.0",  git="https://github.com/embassy-rs/embassy", features = ["defmt"] }

 defmt = "0.3"
 defmt-rtt = "0.4"
@ -25,6 +26,7 @@ fixed-macro = "1.2"
 # for web request example
 reqwless = { version = "0.12.0", features = ["defmt",]}
 serde = { version = "1.0.203", default-features = false, features = ["derive"] }
+postcard = "1.0.0"
 serde-json-core = "0.5.1"

 # for assign resources example
@ -57,10 +59,11 @@ pio = "0.2.1"
 rand = { version = "0.8.5", default-features = false }
 embedded-sdmmc = "0.7.0"

-bt-hci = { version = "0.1.0", default-features = false, features = ["defmt"] }
 hex = { version = "0.4.3", default-features=false }
 mpu6050 = { git = "https://git.ank.dev/ank/mpu6050" }
 vl53l0x = "1.0.1"
+nalgebra = { version = "0.31.2", default-features=false, features = ["serde-serialize-no-std"] }
+common = { path = "../common" }


 [profile.release]
--- a/controller/src/main.rs
+++ b/controller/src/main.rs
@ -5,18 +5,22 @@
 #![no_main]
 #![allow(async_fn_in_trait)]

+mod sensor_manager;

 use core::array;
 use core::borrow::BorrowMut;
 use core::cell::RefCell;
 use core::fmt::Formatter;
+use core::mem::transmute;
 use core::panic::PanicInfo;
 use core::str::from_utf8;

-use bt_hci::cmd::info;
-use cyw43_pio::PioSpi;
+use cyw43_pio::{PioSpi, DEFAULT_CLOCK_DIVIDER};
 use embassy_rp::i2c::{Async, I2c};
+use embassy_rp::multicore::{spawn_core1, Stack};
 use embassy_rp::pwm::{self, Pwm};
+use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
+use embassy_sync::channel::Channel;
 use embedded_hal_bus::i2c::RefCellDevice;
 use fixed::FixedU16;
 use log::*;
@ -30,11 +34,14 @@ 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::{Delay, Duration, Timer};
-use embedded_io_async::Write;
+use embassy_time::{Delay, Duration, Instant, Timer};
+use embedded_io_async::{Read, Write};
 use mpu6050::Mpu6050;
+use nalgebra::clamp;
 use rand::RngCore;
 use reqwless::response;
+use sensor_manager::{sensor_manager, SensorHardware};
+use common::{ControlPacket, SensorData, TelemetryPacket};
 use static_cell::StaticCell;
 use defmt_rtt as _;

@ -45,6 +52,9 @@ bind_interrupts!(struct Irqs {
    USBCTRL_IRQ => embassy_rp::usb::InterruptHandler<USB>;
 });

+static mut CORE1_STACK: Stack<4096> = Stack::new();
+pub static CHANNEL: Channel<CriticalSectionRawMutex, (SensorData, Instant), 1> = Channel::new();
+
 #[embassy_executor::task]
 async fn logger_task(driver: Driver<'static, USB>) {
    embassy_usb_logger::run!(1024, log::LevelFilter::Debug, driver);
@ -79,11 +89,11 @@ async fn main(spawner: Spawner) {
    let mut flipper = Output::new(p.PIN_22, Level::Low);

    let mut c: pwm::Config = Default::default();
-    c.divider = 255.into();
-    c.top = 8715;
-    c.compare_b = 7210;
-    c.compare_a = 2240;
-    let mut pwm = Pwm::new_output_ab(p.PWM_SLICE1, p.PIN_18, p.PIN_19, c.clone());
+    c.divider = 40.into();
+    c.top = 62500; // 20ms
+    c.compare_b = 4687; // 1.5ms
+    c.compare_a = 4687; // 1.5ms
+    let mut drive_pwm = Pwm::new_output_ab(p.PWM_SLICE1, p.PIN_18, p.PIN_19, c.clone());
    flipper.set_high();
    
    let sda = p.PIN_20;
@ -95,47 +105,21 @@ async fn main(spawner: Spawner) {
    let scl = p.PIN_27;
    let config = embassy_rp::i2c::Config::default();
    let bus1 = embassy_rp::i2c::I2c::new_async(p.I2C1, scl, sda, Irqs, config);
-    let bus1 = RefCell::new(bus1);
+    let tof2enable = Output::new(p.PIN_16, Level::Low);

-    info!("made RefCell");
+    let hardware = SensorHardware {
+        bus_tof: unsafe {transmute(bus)},
+        bus_gyro: unsafe {transmute(bus1)},
+        tof2enable,
+    };

-    let mut tof2on = Output::new(p.PIN_16, Level::Low);
-    info!("2");
-    Timer::after_millis(1).await;
-    let mut tof = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus1)).unwrap();
-    info!("3");
-    tof.set_address(0x32).unwrap();
-    info!("4");
-    tof2on.set_high();
-    Timer::after_micros(1200).await; // DS11555 3.2
-    info!("5");
-    let mut tof2 = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus1)).unwrap();
-    info!("6");
-
-
-    let mut gyro = Mpu6050::new_with_addr(bus,0x68);
-
-    Timer::after_millis(1300).await;
-    gyro.init(&mut Delay).unwrap();
-
-    gyro.set_gyro_range(mpu6050::device::GyroRange::D2000).unwrap();
-    gyro.set_accel_range(mpu6050::device::AccelRange::G16).unwrap();
-    info!("pinidtd");
-    Timer::after_millis(1300).await;
-
-    loop {
-        //info!("pos: {}", gyro.get_gyro().unwrap().x);
-        //info!("jerk: {}", gyro.get_acc().unwrap().x);
-        //info!("temp: {}", gyro.get_temp().unwrap() * (9./5.) + 32.);
-        if let Ok(dist) = tof.read_range_single_millimeters_blocking() {
-            info!("dist1: {dist}mm");
+    spawn_core1(
+        p.CORE1, 
+        unsafe { &mut *core::ptr::addr_of_mut!(CORE1_STACK) }, 
+        move || {
+            sensor_manager(hardware)
        }
-        Timer::after_millis(3).await;
-        if let Ok(dist) = tof2.read_range_single_millimeters_blocking() {
-            info!("dist2: {dist}mm");
-        }
-        Timer::after_millis(23).await;
-    }
+    );

    let fw = include_bytes!("../../cyw43-firmware/43439A0.bin");
    let clm = include_bytes!("../../cyw43-firmware/43439A0_clm.bin");
@ -150,7 +134,16 @@ async fn main(spawner: Spawner) {
    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);
+    let spi = PioSpi::new(
+        &mut pio.common,
+        pio.sm0,
+        DEFAULT_CLOCK_DIVIDER,
+        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());
@ -178,311 +171,24 @@ async fn main(spawner: Spawner) {

    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!
+    // password is not terribly private information
+    control.start_ap_wpa2("cruisecontrol", "dxSk2avMFvsY", 5).await;

    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],
-
-    //    [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;
-    ////}
+    let mut telem_buf = [0; 4096];

    loop {
        let mut socket = TcpSocket::new(stack, &mut rx_buffer, &mut tx_buffer);
        socket.set_timeout(Some(Duration::from_secs(3)));

        let mut c: pwm::Config = Default::default();
-        c.divider = 255.into();
-        c.top = 8715;
-        c.compare_b = 0;
-        c.compare_a = 0;
-        pwm.set_config(&c);
+        c.divider = 40.into();
+        c.top = 62500; // 20ms
+        c.compare_b = 4687; // 1.5ms
+        c.compare_a = 4687; // 1.5ms
+        drive_pwm.set_config(&c);

        flipper.set_low();

@ -497,85 +203,41 @@ async fn main(spawner: Spawner) {
        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;
-                },
-                'D' => {
-                    let left: [u8;2] =  hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
-                    let right: [u8;2] =  hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
-                    info!("left {:?}, right {left:?}", right);
-
-                    let left = u16::from_be_bytes(left);
-                    c.compare_a = left;
-                    let right = u16::from_be_bytes(right);
-                    c.compare_b = right;
-                    info!("left {:?}, right {left:?}", right);
-                    pwm.set_config(&c);
-                },
-                'F' => {
-                    flipper.set_high();
-                },
-                'L' => {
-                    flipper.set_low();
-                },
-                _ => {}
-                
+            let mut len: [u8; 4] = [0;4];
+            if let Err(e) = socket.read_exact(&mut len).await {
+                warn!("read err: {e:?}");
+            }
+            let len = u32::from_be_bytes(len) as usize;
+            let data = &mut buf[0..len];
+            if let Err(e) = socket.read_exact(data).await {
+                warn!("read err: {e:?}");
+                break;
            }
            
-            //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;
+            if let Ok(data) = postcard::from_bytes::<ControlPacket>(&data) {
+                info!("got {data:?}");
+                match data {
+                    ControlPacket::Twist(forward, right) => {
+                        info!("left to {}", clamp(forward+right, -1., 1.));
+                        info!("right to {}", clamp(forward-right, -1., 1.));
+                    },
+                    d => {error!("unimplemented: {d:?}")} // TODO
                }
-            };
+            }
+
+            if let Ok((data, _ts)) = CHANNEL.try_receive() {
+                let telem = TelemetryPacket {
+                    sensors: data,
+                    voltage: 0.0, // TODO
+                };
+
+                if let Ok(telem) = postcard::to_slice(&telem, &mut telem_buf) {
+                    if let Err(e) = socket.write_all(&telem).await {
+                        warn!("write error: {:?}", e);
+                        break;
+                    }
+                }
+            }
        }
    }
 }
--- a/controller/src/sensor_manager.rs
+++ b/controller/src/sensor_manager.rs
@ -0,0 +1,111 @@
+use core::sync::atomic::{AtomicU8, Ordering};
+use core::task::Poll;
+use core::{cell::RefCell, mem::transmute};
+
+use embassy_executor::Executor;
+use embassy_rp::peripherals::{I2C0, I2C1};
+use embassy_time::{Delay, Duration, Instant, Timer};
+use embassy_rp::gpio::Output;
+use embassy_rp::i2c::{Async, I2c};
+use embedded_hal_bus::i2c::RefCellDevice;
+use log::info;
+use mpu6050::Mpu6050;
+use nalgebra::Vector3;
+
+use crate::CHANNEL;
+
+pub struct SensorHardware {
+    pub bus_tof: I2c<'static, I2C1, Async>,
+    pub bus_gyro: I2c<'static, I2C0, Async>,
+    pub tof2enable: Output<'static>,
+}
+
+pub fn sensor_manager(mut hardware: SensorHardware) -> ! {
+    let mut executor = Executor::new();
+    let executor: &'static mut Executor = unsafe{transmute(&mut executor)};
+    executor.run(move |spawner| {
+        spawner.spawn(init_sensors(hardware)).unwrap();
+    });
+}
+
+static POLL_STATE: AtomicU8 = AtomicU8::new(PollState::None as u8);
+#[repr(u8)]
+enum PollState {
+    None,
+    Tof1,
+    Tof2,
+    Gyro,
+}
+
+#[derive(Clone, Debug)]
+pub struct SensorData {
+    /// distance, mm
+    tof1: Option<u16>,
+    /// distance, mm
+    tof2: Option<u16>,
+    /// acceleration, rad/s
+    gyro: Option<Vector3<f32>>,
+    /// acceleration, g
+    accel: Option<Vector3<f32>>,
+    updated: Instant,
+}
+
+#[embassy_executor::task]
+async fn init_sensors(mut hardware: SensorHardware) {
+    let bus = RefCell::new(hardware.bus_tof);
+
+    Timer::after_millis(1).await;
+    let mut tof = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).unwrap();
+    tof.set_address(0x32).unwrap();
+    hardware.tof2enable.set_high();
+    Timer::after_micros(1200).await; // DS11555 3.2
+    let mut tof2 = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).unwrap();
+
+    let mut gyro = Mpu6050::new_with_addr(hardware.bus_gyro,0x68);
+    gyro.init(&mut Delay).unwrap();
+    gyro.set_gyro_range(mpu6050::device::GyroRange::D2000).unwrap();
+    gyro.set_accel_range(mpu6050::device::AccelRange::G16).unwrap();
+
+
+    let mut data = SensorData {
+        tof1: None,
+        tof2: None,
+        gyro: None,
+        accel: None,
+        updated: Instant::now(),
+    };
+
+    loop {
+        let mut updated = false;
+        POLL_STATE.store(PollState::Tof1 as u8, Ordering::SeqCst);
+        if let Ok(dist) = tof.read_range_single_millimeters_blocking() {
+            info!("dist1: {dist}mm");
+            data.tof1 = Some(dist);
+            updated = true;
+        }
+        Timer::after_millis(3).await;
+        POLL_STATE.store(PollState::Tof2 as u8, Ordering::SeqCst);
+        if let Ok(dist) = tof2.read_range_single_millimeters_blocking() {
+            info!("dist2: {dist}mm");
+            data.tof2 = Some(dist);
+            updated = true;
+        }
+        Timer::after_millis(3).await;
+        POLL_STATE.store(PollState::Gyro as u8, Ordering::SeqCst);
+        if let Ok(gyro) = gyro.get_gyro() {
+            info!("rotation: {gyro}");
+            data.gyro = Some(gyro);
+            updated = true;
+        }
+        if let Ok(accel) = gyro.get_acc() {
+            data.accel = Some(accel);
+            updated = true;
+        }
+        if updated {
+            data.updated = Instant::now();
+        }
+        POLL_STATE.store(PollState::None as u8, Ordering::SeqCst);
+
+        CHANNEL.send(data.clone()).await;
+    }
+}
--- a/controller/src/vl53l0.rs
+++ b/controller/src/vl53l0.rs
@ -1,91 +0,0 @@
-use core::ops::Deref;
-
-#[repr(u8)]
-pub enum RegAddr
-    {
-      SYSRANGE_START                              = 0x00,
-
-      SYSTEM_THRESH_HIGH                          = 0x0C,
-      SYSTEM_THRESH_LOW                           = 0x0E,
-
-      SYSTEM_SEQUENCE_CONFIG                      = 0x01,
-      SYSTEM_RANGE_CONFIG                         = 0x09,
-      SYSTEM_INTERMEASUREMENT_PERIOD              = 0x04,
-
-      SYSTEM_INTERRUPT_CONFIG_GPIO                = 0x0A,
-
-      GPIO_HV_MUX_ACTIVE_HIGH                     = 0x84,
-
-      SYSTEM_INTERRUPT_CLEAR                      = 0x0B,
-
-      RESULT_INTERRUPT_STATUS                     = 0x13,
-      RESULT_RANGE_STATUS                         = 0x14,
-
-      RESULT_CORE_AMBIENT_WINDOW_EVENTS_RTN       = 0xBC,
-      //RESULT_CORE_RANGING_TOTAL_EVENTS_RTN        = 0xC0,
-      RESULT_CORE_AMBIENT_WINDOW_EVENTS_REF       = 0xD0,
-      RESULT_CORE_RANGING_TOTAL_EVENTS_REF        = 0xD4,
-      RESULT_PEAK_SIGNAL_RATE_REF                 = 0xB6,
-
-      ALGO_PART_TO_PART_RANGE_OFFSET_MM           = 0x28,
-
-      I2C_SLAVE_DEVICE_ADDRESS                    = 0x8A,
-
-      MSRC_CONFIG_CONTROL                         = 0x60,
-
-      PRE_RANGE_CONFIG_MIN_SNR                    = 0x27,
-      PRE_RANGE_CONFIG_VALID_PHASE_LOW            = 0x56,
-      PRE_RANGE_CONFIG_VALID_PHASE_HIGH           = 0x57,
-      PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT          = 0x64,
-
-      FINAL_RANGE_CONFIG_MIN_SNR                  = 0x67,
-      FINAL_RANGE_CONFIG_VALID_PHASE_LOW          = 0x47,
-      FINAL_RANGE_CONFIG_VALID_PHASE_HIGH         = 0x48,
-      FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT = 0x44,
-
-      PRE_RANGE_CONFIG_SIGMA_THRESH_HI            = 0x61,
-      PRE_RANGE_CONFIG_SIGMA_THRESH_LO            = 0x62,
-
-      PRE_RANGE_CONFIG_VCSEL_PERIOD               = 0x50,
-      PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI          = 0x51,
-      PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO          = 0x52,
-
-      SYSTEM_HISTOGRAM_BIN                        = 0x81,
-      HISTOGRAM_CONFIG_INITIAL_PHASE_SELECT       = 0x33,
-      HISTOGRAM_CONFIG_READOUT_CTRL               = 0x55,
-
-      FINAL_RANGE_CONFIG_VCSEL_PERIOD             = 0x70,
-      FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI        = 0x71,
-      FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO        = 0x72,
-      CROSSTALK_COMPENSATION_PEAK_RATE_MCPS       = 0x20,
-
-      MSRC_CONFIG_TIMEOUT_MACROP                  = 0x46,
-
-      SOFT_RESET_GO2_SOFT_RESET_N                 = 0xBF,
-      IDENTIFICATION_MODEL_ID                     = 0xC0,
-      IDENTIFICATION_REVISION_ID                  = 0xC2,
-
-      OSC_CALIBRATE_VAL                           = 0xF8,
-
-      GLOBAL_CONFIG_VCSEL_WIDTH                   = 0x32,
-      GLOBAL_CONFIG_SPAD_ENABLES_REF_0            = 0xB0,
-      GLOBAL_CONFIG_SPAD_ENABLES_REF_1            = 0xB1,
-      GLOBAL_CONFIG_SPAD_ENABLES_REF_2            = 0xB2,
-      GLOBAL_CONFIG_SPAD_ENABLES_REF_3            = 0xB3,
-      GLOBAL_CONFIG_SPAD_ENABLES_REF_4            = 0xB4,
-      GLOBAL_CONFIG_SPAD_ENABLES_REF_5            = 0xB5,
-
-      //GLOBAL_CONFIG_REF_EN_START_SELECT           = 0xB6,
-      DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD         = 0x4E,
-      DYNAMIC_SPAD_REF_EN_START_OFFSET            = 0x4F,
-      POWER_MANAGEMENT_GO1_POWER_FORCE            = 0x80,
-
-      VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV           = 0x89,
-
-      ALGO_PHASECAL_LIM                           = 0x30,
-      //ALGO_PHASECAL_CONFIG_TIMEOUT                = 0x30,
-}
-
-struct Vl53l0x { 
-    
-}
--- a/interface/.gitignore
+++ b/interface/.gitignore
@ -0,0 +1 @@
+target
Author	SHA1	Message	Date
Andy Killorin	18f8c24021	rx netcode	2025-01-18 14:26:13 -05:00
Andy Killorin	736584f5d6	copy pwm config from door winch	2025-01-18 13:45:49 -05:00
Andy Killorin	3b829b1b59	remove partial tof driver	2025-01-18 13:31:25 -05:00
Andy Killorin	9f740d8435	sensor data timestamp	2025-01-18 13:31:10 -05:00
Andy Killorin	088e0225ca	feasible second core sensor impl	2025-01-18 13:27:48 -05:00
Andy Killorin	7a918e2b65	transitioning sensor reads to second core	2025-01-18 13:10:07 -05:00