ank/cruisecontrol
1
Fork 0
Code Issues Activity

Compare commits

base: ank:4c616de4a0688354de10d4247211274f9d4817ef
Branches Tags
ank:pico
ank:half-duplex
ank:25c-antweight-comp
..
compare: ank:18f8c24021aac868b6188be2f22bb504c7831399
Branches Tags
ank:pico
ank:half-duplex
ank:25c-antweight-comp

6 commits
4c616de4a0 ... 18f8c24021

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
6 changed files with 656 additions and 699 deletions
Show stats Expand all files Collapse all files

633
controller/Cargo.lock generated
View file

File diff suppressed because it is too large Load diff

23
controller/Cargo.toml
View file

@ -6,16 +6,17 @@ license = "MIT OR Apache-2.0"
[dependencies] [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-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.3.2", git="https://github.com/embassy-rs/embassy", features = ["defmt", "defmt-timestamp-uptime"] } embassy-time = { version = "0.4.0", 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-rp = { version = "0.3.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-sync = { version = "0.6.2", git="https://github.com/embassy-rs/embassy" }
embassy-net = { version = "0.4.0", git="https://github.com/embassy-rs/embassy", features = ["defmt", "tcp", "udp", "raw", "dhcpv4", "medium-ethernet", "dns"] } embassy-usb = { version = "0.4.0", git="https://github.com/embassy-rs/embassy", features = ["defmt"] }
embassy-net-wiznet = { version = "0.1.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-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" } embassy-usb-logger = { version = "0.4.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 = { version = "0.3.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"] } cyw43-pio = { version = "0.3.0", git="https://github.com/embassy-rs/embassy", features = ["defmt"] }
defmt = "0.3" defmt = "0.3"
defmt-rtt = "0.4" defmt-rtt = "0.4"
@ -25,6 +26,7 @@ fixed-macro = "1.2"
# for web request example # for web request example
reqwless = { version = "0.12.0", features = ["defmt",]} reqwless = { version = "0.12.0", features = ["defmt",]}
serde = { version = "1.0.203", default-features = false, features = ["derive"] } serde = { version = "1.0.203", default-features = false, features = ["derive"] }
postcard = "1.0.0"
serde-json-core = "0.5.1" serde-json-core = "0.5.1"
# for assign resources example # for assign resources example
@ -57,10 +59,11 @@ pio = "0.2.1"
rand = { version = "0.8.5", default-features = false } rand = { version = "0.8.5", default-features = false }
embedded-sdmmc = "0.7.0" embedded-sdmmc = "0.7.0"
bt-hci = { version = "0.1.0", default-features = false, features = ["defmt"] }
hex = { version = "0.4.3", default-features=false } hex = { version = "0.4.3", default-features=false }
mpu6050 = { git = "https://git.ank.dev/ank/mpu6050" } mpu6050 = { git = "https://git.ank.dev/ank/mpu6050" }
vl53l0x = "1.0.1" vl53l0x = "1.0.1"
nalgebra = { version = "0.31.2", default-features=false, features = ["serde-serialize-no-std"] }
common = { path = "../common" }
[profile.release] [profile.release]

490
controller/src/main.rs
View file

@ -5,18 +5,22 @@
#![no_main] #![no_main]
#![allow(async_fn_in_trait)] #![allow(async_fn_in_trait)]
mod sensor_manager;
use core::array; use core::array;
use core::borrow::BorrowMut; use core::borrow::BorrowMut;
use core::cell::RefCell; use core::cell::RefCell;
use core::fmt::Formatter; use core::fmt::Formatter;
use core::mem::transmute;
use core::panic::PanicInfo; use core::panic::PanicInfo;
use core::str::from_utf8; use core::str::from_utf8;
use bt_hci::cmd::info; use cyw43_pio::{PioSpi, DEFAULT_CLOCK_DIVIDER};
use cyw43_pio::PioSpi;
use embassy_rp::i2c::{Async, I2c}; use embassy_rp::i2c::{Async, I2c};
use embassy_rp::multicore::{spawn_core1, Stack};
use embassy_rp::pwm::{self, Pwm}; 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 embedded_hal_bus::i2c::RefCellDevice;
use fixed::FixedU16; use fixed::FixedU16;
use log::*; use log::*;
@ -30,11 +34,14 @@ use embassy_rp::gpio::{Level, Output};
use embassy_rp::peripherals::{DMA_CH0, PIO0, USB}; use embassy_rp::peripherals::{DMA_CH0, PIO0, USB};
use embassy_rp::pio::{InterruptHandler, Pio}; use embassy_rp::pio::{InterruptHandler, Pio};
use embassy_rp::usb::Driver; use embassy_rp::usb::Driver;
use embassy_time::{Delay, Duration, Timer}; use embassy_time::{Delay, Duration, Instant, Timer};
use embedded_io_async::Write; use embedded_io_async::{Read, Write};
use mpu6050::Mpu6050; use mpu6050::Mpu6050;
use nalgebra::clamp;
use rand::RngCore; use rand::RngCore;
use reqwless::response; use reqwless::response;
use sensor_manager::{sensor_manager, SensorHardware};
use common::{ControlPacket, SensorData, TelemetryPacket};
use static_cell::StaticCell; use static_cell::StaticCell;
use defmt_rtt as _; use defmt_rtt as _;
@ -45,6 +52,9 @@ bind_interrupts!(struct Irqs {
USBCTRL_IRQ => embassy_rp::usb::InterruptHandler<USB>; 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] #[embassy_executor::task]
async fn logger_task(driver: Driver<'static, USB>) { async fn logger_task(driver: Driver<'static, USB>) {
embassy_usb_logger::run!(1024, log::LevelFilter::Debug, driver); 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 flipper = Output::new(p.PIN_22, Level::Low);
let mut c: pwm::Config = Default::default(); let mut c: pwm::Config = Default::default();
c.divider = 255.into(); c.divider = 40.into();
c.top = 8715; c.top = 62500; // 20ms
c.compare_b = 7210; c.compare_b = 4687; // 1.5ms
c.compare_a = 2240; c.compare_a = 4687; // 1.5ms
let mut pwm = Pwm::new_output_ab(p.PWM_SLICE1, p.PIN_18, p.PIN_19, c.clone()); let mut drive_pwm = Pwm::new_output_ab(p.PWM_SLICE1, p.PIN_18, p.PIN_19, c.clone());
flipper.set_high(); flipper.set_high();
let sda = p.PIN_20; let sda = p.PIN_20;
@ -95,47 +105,21 @@ async fn main(spawner: Spawner) {
let scl = p.PIN_27; let scl = p.PIN_27;
let config = embassy_rp::i2c::Config::default(); let config = embassy_rp::i2c::Config::default();
let bus1 = embassy_rp::i2c::I2c::new_async(p.I2C1, scl, sda, Irqs, config); 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); spawn_core1(
info!("2"); p.CORE1,
Timer::after_millis(1).await; unsafe { &mut *core::ptr::addr_of_mut!(CORE1_STACK) },
let mut tof = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus1)).unwrap(); move || {
info!("3"); sensor_manager(hardware)
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");
}
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 fw = include_bytes!("../../cyw43-firmware/43439A0.bin");
let clm = include_bytes!("../../cyw43-firmware/43439A0_clm.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 pwr = Output::new(p.PIN_23, Level::Low);
let cs = Output::new(p.PIN_25, Level::High); let cs = Output::new(p.PIN_25, Level::High);
let mut pio = Pio::new(p.PIO0, Irqs); 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(); static STATE: StaticCell<cyw43::State> = StaticCell::new();
let state = STATE.init(cyw43::State::new()); let state = STATE.init(cyw43::State::new());
@ -178,311 +171,24 @@ async fn main(spawner: Spawner) {
defmt::unwrap!(spawner.spawn(net_task(runner))); defmt::unwrap!(spawner.spawn(net_task(runner)));
//control.start_ap_open("cyw43", 5).await; // password is not terribly private information
control.start_ap_wpa2("cyw43", "password", 5).await; control.start_ap_wpa2("cruisecontrol", "dxSk2avMFvsY", 5).await;
// And now we can use it!
let mut rx_buffer = [0; 4096]; let mut rx_buffer = [0; 4096];
let mut tx_buffer = [0; 4096]; let mut tx_buffer = [0; 4096];
let mut buf = [0; 4096]; let mut buf = [0; 4096];
let mut telem_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;
////}
loop { loop {
let mut socket = TcpSocket::new(stack, &mut rx_buffer, &mut tx_buffer); let mut socket = TcpSocket::new(stack, &mut rx_buffer, &mut tx_buffer);
socket.set_timeout(Some(Duration::from_secs(3))); socket.set_timeout(Some(Duration::from_secs(3)));
let mut c: pwm::Config = Default::default(); let mut c: pwm::Config = Default::default();
c.divider = 255.into(); c.divider = 40.into();
c.top = 8715; c.top = 62500; // 20ms
c.compare_b = 0; c.compare_b = 4687; // 1.5ms
c.compare_a = 0; c.compare_a = 4687; // 1.5ms
pwm.set_config(&c); drive_pwm.set_config(&c);
flipper.set_low(); flipper.set_low();
@ -497,85 +203,41 @@ async fn main(spawner: Spawner) {
control.gpio_set(0, true).await; control.gpio_set(0, true).await;
loop { loop {
let mut n = match socket.read(&mut buf).await { let mut len: [u8; 4] = [0;4];
Ok(0) => { if let Err(e) = socket.read_exact(&mut len).await {
warn!("read EOF"); 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; break;
} }
Ok(n) => n,
Err(e) => { if let Ok(data) = postcard::from_bytes::<ControlPacket>(&data) {
warn!("read error: {:?}", e); info!("got {data:?}");
break; 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) {
info!("rxd {}", from_utf8(&buf[..n]).unwrap()); if let Err(e) = socket.write_all(&telem).await {
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 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); warn!("write error: {:?}", e);
break; break;
} }
}; }
}
} }
} }
} }

111
controller/src/sensor_manager.rs Normal file
View file

@ -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;
}
}

91
controller/src/vl53l0.rs
View file

@ -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 {
}

1
interface/.gitignore vendored Normal file
View file

@ -0,0 +1 @@
target