transitioning sensor reads to second core

controller/Cargo.lock

@@ -306,6 +306,7 @@ dependencies = [
  "hex",
  "log",
  "mpu6050",
+ "nalgebra",
  "panic-probe",
  "pio",
  "pio-proc",

controller/Cargo.toml

@@ -9,6 +9,7 @@ license = "MIT OR Apache-2.0"
 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-sync = { version = "0.6.2", git="https://github.com/embassy-rs/embassy" }
 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"] }
@@ -61,6 +62,7 @@ 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 }
 
 
 [profile.release]

controller/src/main.rs

@@ -5,6 +5,7 @@
 #![no_main]
 #![allow(async_fn_in_trait)]
 
+mod sensor_manager;
 
 use core::array;
 use core::borrow::BorrowMut;
@@ -16,6 +17,7 @@ use core::str::from_utf8;
 use bt_hci::cmd::info;
 use cyw43_pio::PioSpi;
 use embassy_rp::i2c::{Async, I2c};
+use embassy_rp::multicore::spawn_core1;
 use embassy_rp::pwm::{self, Pwm};
 use embedded_hal_bus::i2c::RefCellDevice;
 use fixed::FixedU16;
@@ -35,6 +37,7 @@ use embedded_io_async::Write;
 use mpu6050::Mpu6050;
 use rand::RngCore;
 use reqwless::response;
+use sensor_manager::{sensor_manager, SensorData};
 use static_cell::StaticCell;
 use defmt_rtt as _;
 
@@ -45,6 +48,9 @@ bind_interrupts!(struct Irqs {
     USBCTRL_IRQ => embassy_rp::usb::InterruptHandler<USB>;
 });
 
+static mut CORE1_STACK: Stack<4096> = Stack::new();
+static CHANNEL: Channel<CriticalSectionRawMutex, SensorData, 1> = Channel::new();
+
 #[embassy_executor::task]
 async fn logger_task(driver: Driver<'static, USB>) {
     embassy_usb_logger::run!(1024, log::LevelFilter::Debug, driver);
@@ -96,17 +102,26 @@ async fn main(spawner: Spawner) {
     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 mut tof2enable = Output::new(p.PIN_16, Level::Low);
+
+
+    spawn_core1(
+        p.CORE1, 
+        unsafe { &mut *core::ptr::addr_of_mut!(CORE1_STACK) }, 
+        move || {
+            sensor_manager(bus, tof2enable)
+        }
+    );
 
     info!("made RefCell");
 
-    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();
+    tof2enable.set_high();
     Timer::after_micros(1200).await; // DS11555 3.2
     info!("5");
     let mut tof2 = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus1)).unwrap();
@@ -179,7 +194,7 @@ 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;
+    control.start_ap_wpa2("lovesense_setup2", "password123", 5).await;
 
     // And now we can use it!
 

controller/src/sensor_manager.rs

@@ -0,0 +1,102 @@
+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;
+
+pub struct SensorHardware {
+    bus_tof: I2c<'static, I2C1, Async>,
+    bus_gyro: I2c<'static, I2C0, Async>,
+    tof2enable: Output<'static>,
+}
+
+pub fn sensor_manager(mut hardware: SensorHardware) -> ! {
+    let executor = Executor::new();
+    let executor: &'static mut Executor = unsafe{transmute(&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,
+}
+
+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 {
+        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);
+        }
+        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);
+        }
+        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);
+        }
+        if let Ok(accel) = gyro.get_acc() {
+            data.accel = Some(accel);
+        }
+        POLL_STATE.store(PollState::None as u8, Ordering::SeqCst);
+
+
+
+        
+    }
+}

interface/.gitignore

@@ -0,0 +1 @@
+target