added deadzone to gamepad controller mended drive polarity wait to start telem until awakened by northbridge (linux doesn't boot in 300 cycles)
80 lines
2.1 KiB
Rust
80 lines
2.1 KiB
Rust
#![feature(async_closure)]
|
|
|
|
use anyhow::{Context, Result};
|
|
use common::{Command, Response, BAUDRATE};
|
|
use framed_codec::FramedCodec;
|
|
use futures::{SinkExt, StreamExt};
|
|
use tokio::{io::AsyncReadExt, net::{TcpListener, TcpSocket}, sync::{broadcast, watch::{self, Sender}}};
|
|
use tokio_serial::SerialPortBuilderExt;
|
|
use tokio_util::codec::Framed;
|
|
|
|
mod framed_codec;
|
|
|
|
#[tokio::main]
|
|
async fn main() -> Result<()> {
|
|
let mut serial = tokio_serial::new("/dev/ttyAMA0", BAUDRATE).open_native_async()?;
|
|
|
|
let (sensor_sender, sensor_data) = broadcast::channel(5);
|
|
|
|
serial.set_exclusive(false)?;
|
|
|
|
let (mut write, read) = Framed::new(serial, FramedCodec::new()).split();
|
|
|
|
let mut read = read.map(|data| -> Result<Response> {
|
|
Ok(postcard::from_bytes::<Response>(&data.ok().context("decode err")?)?)
|
|
});
|
|
|
|
let (send, commands) = watch::channel(Command::Stop);
|
|
|
|
tokio::spawn(control(send));
|
|
|
|
println!("starting");
|
|
write.send(Command::Enable).await?;
|
|
write.flush().await?;
|
|
|
|
loop {
|
|
let Some(Ok(data)) = read.next().await else {
|
|
continue;
|
|
};
|
|
if data.enabled {
|
|
write.send(Command::FeedWatchdog).await?;
|
|
write.flush().await?;
|
|
|
|
} else {
|
|
println!("enabled southbridge");
|
|
write.send(Command::Enable).await?;
|
|
}
|
|
|
|
write.send(Command::Enable).await?;
|
|
write.flush().await?;
|
|
|
|
//dbg!(&data);
|
|
|
|
if let Some(data) = data.sensor_data {
|
|
let _ = sensor_sender.send(data);
|
|
}
|
|
|
|
write.send(dbg!(commands.borrow().clone())).await?;
|
|
write.flush().await?;
|
|
}
|
|
}
|
|
|
|
async fn control(sender: Sender<Command>) -> Result<()> {
|
|
let listener = TcpListener::bind("0.0.0.0:3322").await?;
|
|
|
|
let (mut stream, addr) = listener.accept().await?;
|
|
|
|
println!("connected to {addr:?}");
|
|
|
|
loop {
|
|
let len = stream.read_u32().await?;
|
|
let mut buf = vec![0; len as usize];
|
|
stream.read_exact(&mut buf).await?;
|
|
|
|
let cmd: Command = postcard::from_bytes(&buf)?;
|
|
|
|
println!("recv {cmd:?}");
|
|
|
|
sender.send(cmd)?;
|
|
}
|
|
}
|