feasible second core sensor impl
This commit is contained in:
parent
7a918e2b65
commit
088e0225ca
GPG key ID:
23F9463ECB67FE8C
4 changed files with 463 additions and 274 deletions
601
controller/Cargo.lock
generated
601
controller/Cargo.lock
generated
File diff suppressed because it is too large
Load diff
|
|
@ -6,17 +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-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.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-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"
|
||||
|
|
@ -58,7 +58,6 @@ 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"
|
||||
|
|
|
|||
|
|
@ -11,14 +11,16 @@ 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;
|
||||
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::*;
|
||||
|
|
@ -37,7 +39,7 @@ use embedded_io_async::Write;
|
|||
use mpu6050::Mpu6050;
|
||||
use rand::RngCore;
|
||||
use reqwless::response;
|
||||
use sensor_manager::{sensor_manager, SensorData};
|
||||
use sensor_manager::{sensor_manager, SensorData, SensorHardware};
|
||||
use static_cell::StaticCell;
|
||||
use defmt_rtt as _;
|
||||
|
||||
|
|
@ -49,7 +51,7 @@ bind_interrupts!(struct Irqs {
|
|||
});
|
||||
|
||||
static mut CORE1_STACK: Stack<4096> = Stack::new();
|
||||
static CHANNEL: Channel<CriticalSectionRawMutex, SensorData, 1> = Channel::new();
|
||||
pub static CHANNEL: Channel<CriticalSectionRawMutex, SensorData, 1> = Channel::new();
|
||||
|
||||
#[embassy_executor::task]
|
||||
async fn logger_task(driver: Driver<'static, USB>) {
|
||||
|
|
@ -101,57 +103,22 @@ 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 mut tof2enable = Output::new(p.PIN_16, Level::Low);
|
||||
let tof2enable = Output::new(p.PIN_16, Level::Low);
|
||||
|
||||
let hardware = SensorHardware {
|
||||
bus_tof: unsafe {transmute(bus)},
|
||||
bus_gyro: unsafe {transmute(bus1)},
|
||||
tof2enable,
|
||||
};
|
||||
|
||||
spawn_core1(
|
||||
p.CORE1,
|
||||
unsafe { &mut *core::ptr::addr_of_mut!(CORE1_STACK) },
|
||||
move || {
|
||||
sensor_manager(bus, tof2enable)
|
||||
sensor_manager(hardware)
|
||||
}
|
||||
);
|
||||
|
||||
info!("made RefCell");
|
||||
|
||||
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");
|
||||
tof2enable.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");
|
||||
}
|
||||
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");
|
||||
|
||||
|
|
@ -165,7 +132,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());
|
||||
|
|
@ -531,32 +507,6 @@ async fn main(spawner: Spawner) {
|
|||
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();
|
||||
|
|
|
|||
|
|
@ -12,15 +12,17 @@ use log::info;
|
|||
use mpu6050::Mpu6050;
|
||||
use nalgebra::Vector3;
|
||||
|
||||
use crate::CHANNEL;
|
||||
|
||||
pub struct SensorHardware {
|
||||
bus_tof: I2c<'static, I2C1, Async>,
|
||||
bus_gyro: I2c<'static, I2C0, Async>,
|
||||
tof2enable: Output<'static>,
|
||||
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 executor = Executor::new();
|
||||
let executor: &'static mut Executor = unsafe{transmute(&executor)};
|
||||
let mut executor = Executor::new();
|
||||
let executor: &'static mut Executor = unsafe{transmute(&mut executor)};
|
||||
executor.run(move |spawner| {
|
||||
spawner.spawn(init_sensors(hardware)).unwrap();
|
||||
});
|
||||
|
|
@ -35,7 +37,8 @@ enum PollState {
|
|||
Gyro,
|
||||
}
|
||||
|
||||
struct SensorData {
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct SensorData {
|
||||
/// distance, mm
|
||||
tof1: Option<u16>,
|
||||
/// distance, mm
|
||||
|
|
@ -95,8 +98,6 @@ async fn init_sensors(mut hardware: SensorHardware) {
|
|||
}
|
||||
POLL_STATE.store(PollState::None as u8, Ordering::SeqCst);
|
||||
|
||||
|
||||
|
||||
|
||||
CHANNEL.send(data.clone()).await;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue