ank/cruisecontrol
1
Fork 0
Code Issues Activity

rx netcode

Browse source
This commit is contained in:
Andy Killorin 2025-01-18 14:26:13 -05:00
parent 736584f5d6
commit 4945b4d777
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
4 changed files with 79 additions and 73 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

31
controller/Cargo.lock generated
View file

@ -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"

4
controller/Cargo.toml
View file

@ -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]

101
controller/src/main.rs
View file

@ -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<CriticalSectionRawMutex, SensorData, 1> = Channel::new();
pub static CHANNEL: Channel<CriticalSectionRawMutex, (SensorData, Instant), 1> = 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;
}
};
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();
},
_ => {}
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;
}
//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);
break;
if let Ok(data) = postcard::from_bytes::<ControlPacket>(&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
}
};
}
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) {
if let Err(e) = socket.write_all(&telem).await {
warn!("write error: {:?}", e);
break;
}
}
}
}
}
}

20
controller/src/sensor_manager.rs
View file

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