From 7a918e2b65cd559a7cdec726d94194e201bd3993 Mon Sep 17 00:00:00 2001 From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com> Date: Sat, 18 Jan 2025 13:10:07 -0500 Subject: [PATCH] transitioning sensor reads to second core --- controller/Cargo.lock | 1 + controller/Cargo.toml | 2 + controller/src/main.rs | 21 ++++++- controller/src/sensor_manager.rs | 102 +++++++++++++++++++++++++++++++ interface/.gitignore | 1 + 5 files changed, 124 insertions(+), 3 deletions(-) create mode 100644 controller/src/sensor_manager.rs create mode 100644 interface/.gitignore diff --git a/controller/Cargo.lock b/controller/Cargo.lock index 13bdae2..87ba8f8 100644 --- a/controller/Cargo.lock +++ b/controller/Cargo.lock @@ -306,6 +306,7 @@ dependencies = [ "hex", "log", "mpu6050", + "nalgebra", "panic-probe", "pio", "pio-proc", diff --git a/controller/Cargo.toml b/controller/Cargo.toml index 5eba3f7..bffed1a 100644 --- a/controller/Cargo.toml +++ b/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] diff --git a/controller/src/main.rs b/controller/src/main.rs index 4a87d37..d40e121 100644 --- a/controller/src/main.rs +++ b/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; }); +static mut CORE1_STACK: Stack<4096> = Stack::new(); +static CHANNEL: Channel = 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! diff --git a/controller/src/sensor_manager.rs b/controller/src/sensor_manager.rs new file mode 100644 index 0000000..aa475ab --- /dev/null +++ b/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, + /// distance, mm + tof2: Option, + /// acceleration, rad/s + gyro: Option>, + /// acceleration, g + accel: Option>, + 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); + + + + + } +} diff --git a/interface/.gitignore b/interface/.gitignore new file mode 100644 index 0000000..eb5a316 --- /dev/null +++ b/interface/.gitignore @@ -0,0 +1 @@ +target