From 4945b4d7778686d3d87e52524e17647249afb610 Mon Sep 17 00:00:00 2001 From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com> Date: Sat, 18 Jan 2025 14:26:13 -0500 Subject: [PATCH] rx netcode --- controller/Cargo.lock | 31 ++++++++++ controller/Cargo.toml | 4 +- controller/src/main.rs | 97 ++++++++++++++------------------ controller/src/sensor_manager.rs | 20 ++----- 4 files changed, 79 insertions(+), 73 deletions(-) diff --git a/controller/Cargo.lock b/controller/Cargo.lock index e64dc10..dcece99 100644 --- a/controller/Cargo.lock +++ b/controller/Cargo.lock @@ -268,6 +268,12 @@ dependencies = [ "inout", ] +[[package]] +name = "cobs" +version = "0.2.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "67ba02a97a2bd10f4b59b25c7973101c79642302776489e030cd13cdab09ed15" + [[package]] name = "codespan-reporting" version = "0.11.1" @@ -278,6 +284,15 @@ dependencies = [ "unicode-width", ] +[[package]] +name = "common" +version = "0.1.0" +dependencies = [ + "nalgebra", + "postcard", + "serde", +] + [[package]] name = "const-oid" version = "0.9.6" @@ -290,6 +305,7 @@ version = "0.1.0" dependencies = [ "assign-resources", "byte-slice-cast 1.2.2", + "common", "cortex-m", "cortex-m-rt", "critical-section", @@ -326,6 +342,7 @@ dependencies = [ "pio 0.2.1 (registry+https://github.com/rust-lang/crates.io-index)", "pio-proc 0.2.2 (registry+https://github.com/rust-lang/crates.io-index)", "portable-atomic", + "postcard", "rand", "reqwless", "serde", @@ -1375,6 +1392,7 @@ dependencies = [ "atomic-polyfill", "hash32 0.2.1", "rustc_version 0.4.1", + "serde", "spin", "stable_deref_trait", ] @@ -1633,6 +1651,7 @@ dependencies = [ "num-complex", "num-rational", "num-traits", + "serde", "simba", "typenum", ] @@ -1680,6 +1699,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495" dependencies = [ "num-traits", + "serde", ] [[package]] @@ -1944,6 +1964,17 @@ dependencies = [ "critical-section", ] +[[package]] +name = "postcard" +version = "1.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "170a2601f67cc9dba8edd8c4870b15f71a6a2dc196daec8c83f72b59dff628a8" +dependencies = [ + "cobs", + "heapless 0.7.17", + "serde", +] + [[package]] name = "ppv-lite86" version = "0.2.20" diff --git a/controller/Cargo.toml b/controller/Cargo.toml index d9a5555..1692ae5 100644 --- a/controller/Cargo.toml +++ b/controller/Cargo.toml @@ -26,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 @@ -61,7 +62,8 @@ embedded-sdmmc = "0.7.0" 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 } +nalgebra = { version = "0.31.2", default-features=false, features = ["serde-serialize-no-std"] } +common = { path = "../common" } [profile.release] diff --git a/controller/src/main.rs b/controller/src/main.rs index 135bb42..b02531a 100644 --- a/controller/src/main.rs +++ b/controller/src/main.rs @@ -34,12 +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, SensorData, SensorHardware}; +use sensor_manager::{sensor_manager, SensorHardware}; +use common::{ControlPacket, SensorData, TelemetryPacket}; use static_cell::StaticCell; use defmt_rtt as _; @@ -51,7 +53,7 @@ bind_interrupts!(struct Irqs { }); static mut CORE1_STACK: Stack<4096> = Stack::new(); -pub static CHANNEL: Channel = Channel::new(); +pub static CHANNEL: Channel = Channel::new(); #[embassy_executor::task] async fn logger_task(driver: Driver<'static, USB>) { @@ -175,16 +177,17 @@ async fn main(spawner: Spawner) { let mut rx_buffer = [0; 4096]; let mut tx_buffer = [0; 4096]; let mut buf = [0; 4096]; + 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; + 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(); @@ -200,59 +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; - } - }; + 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; + } - - 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() { - '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); - drive_pwm.set_config(&c); - }, - 'F' => { - flipper.set_high(); - }, - 'L' => { - flipper.set_low(); - }, - _ => {} - + if let Ok(data) = postcard::from_bytes::(&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 + } } - //let mut response: [u8;2] = [0;2]; + if let Ok((data, _ts)) = CHANNEL.try_receive() { + let telem = TelemetryPacket { + sensors: data, + voltage: 0.0, // TODO + }; - //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(telem) = postcard::to_slice(&telem, &mut telem_buf) { + if let Err(e) = socket.write_all(&telem).await { + warn!("write error: {:?}", e); + break; + } } - }; + } } } } diff --git a/controller/src/sensor_manager.rs b/controller/src/sensor_manager.rs index 2909ede..4da621b 100644 --- a/controller/src/sensor_manager.rs +++ b/controller/src/sensor_manager.rs @@ -2,6 +2,7 @@ use core::sync::atomic::{AtomicU8, Ordering}; use core::task::Poll; use core::{cell::RefCell, mem::transmute}; +use common::SensorData; use embassy_executor::Executor; use embassy_rp::peripherals::{I2C0, I2C1}; use embassy_time::{Delay, Duration, Instant, Timer}; @@ -37,19 +38,6 @@ enum PollState { Gyro, } -#[derive(Clone, Debug)] -pub 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); @@ -72,8 +60,8 @@ async fn init_sensors(mut hardware: SensorHardware) { tof2: None, gyro: None, accel: None, - updated: Instant::now(), }; + let mut updated_ts = Instant::now(); loop { let mut updated = false; @@ -102,10 +90,10 @@ async fn init_sensors(mut hardware: SensorHardware) { updated = true; } if updated { - data.updated = Instant::now(); + updated_ts = Instant::now(); } POLL_STATE.store(PollState::None as u8, Ordering::SeqCst); - CHANNEL.send(data.clone()).await; + CHANNEL.send((data.clone(), updated_ts.clone())).await; } }