AFG/radomctld
3
0
Fork 0
Code Issues 1 Pull requests Projects Releases Packages Wiki Activity

First end-to-end movement

Browse source
This commit is contained in:
Sebastian 2024-12-04 18:25:26 +01:00
parent f7483fe42a
commit 1498fd27ff
6 changed files with 436 additions and 92 deletions
Download patch file Download diff file Expand all files Collapse all files

5
daemon/Cargo.toml
View file

@ -24,3 +24,8 @@ libusb1-sys = "0.6"
rusb = "0.9"
clap = { version = "4.5.19", features = ["derive"] }
indicatif = "0.17.8"
tokio-serial = {version = "5.4.4", features = ["codec", "rt"] }
tokio-util = { version = "0.7.13", features = ["codec", "rt"] }
bytes = "1.9.0"
futures-util = "0.3.31"
futures = "0.3.31"

68
daemon/src/main.rs
View file

@ -1,7 +1,16 @@
use anyhow::Result;
use anyhow::{anyhow, Context};
use axum::{
extract::State,
http::StatusCode,
routing::{get, post},
Json, Router,
};
use clap::Parser;
use fern::colors::{Color, ColoredLevelConfig};
use log::{debug, error, info, warn, Level};
use serde_json::{json, Value};
use std::time::Duration;
use std::{borrow::Borrow, io};
use tokio::{
self,
@ -10,13 +19,7 @@ use tokio::{
sync::{mpsc, watch},
task::JoinSet,
};
use axum::{
extract::State,
http::StatusCode,
routing::{get, post},
Json, Router,
};
use tokio_serial;
use tower_http::{
services::{ServeDir, ServeFile},
trace::TraceLayer,
@ -86,16 +89,61 @@ struct AxumAppState {
pos_rx: watch::Receiver<(f32, f32)>,
}
#[derive(Parser)]
struct Cli {
/// The usb serial number of the radom-controller
#[arg(short, long)]
serialnumber: String,
/// Listen address for the webserver
#[arg(short, long, default_value = "0.0.0.0:8000")]
web_listen_address: String,
/// Listen address for rotctl
#[arg(short, long, default_value = "0.0.0.0:1337")]
rotctl_listen_address: String,
}
#[tokio::main]
async fn main() -> Result<()> {
setup_logger()?;
let args = Cli::parse();
let ports = tokio_serial::available_ports().unwrap_or(Vec::<serialport::SerialPortInfo>::new());
let mut radom_port: Option<String> = None;
for port in ports {
match port.port_type {
serialport::SerialPortType::UsbPort(usb_port_info) => {
match usb_port_info.serial_number {
Some(serial) => {
debug!("Found a serial port with: {}", serial);
if serial == args.serialnumber {
radom_port = Some(port.port_name.to_owned());
info!("Found radom-controller as {}", port.port_name)
}
}
None => continue,
}
}
_ => continue,
}
}
let radom_port = match radom_port {
Some(port) => port,
_ => {
return Err(anyhow!("No matching port found."));
}
};
let (cmd_tx, cmd_rx) = mpsc::channel::<Command>(16);
let (pos_tx, pos_rx) = watch::channel::<(f32, f32)>((0.0, 0.0));
let mut tasks = JoinSet::new();
tasks.spawn(async move { control_rotor(cmd_rx, pos_tx).await });
tasks.spawn(async move { control_rotor(cmd_rx, pos_tx, radom_port).await });
let state = AxumAppState {
pos_rx: pos_rx.clone(),
@ -108,14 +156,14 @@ async fn main() -> Result<()> {
.with_state(state)
.layer(TraceLayer::new_for_http());
let listener = tokio::net::TcpListener::bind("0.0.0.0:8000").await?;
let listener = tokio::net::TcpListener::bind(args.web_listen_address).await?;
axum::serve(listener, app).await?;
Ok(())
});
tasks.spawn(async move {
let listener = TcpListener::bind("127.0.0.1:1337").await?;
let listener = TcpListener::bind(args.rotctl_listen_address).await?;
loop {
let (socket, _) = listener.accept().await?;

82
daemon/src/rotor.rs
View file

@ -1,5 +1,11 @@
use anyhow::Result;
use bytes::{BufMut, BytesMut};
use futures::{stream::StreamExt, SinkExt};
use log::{debug, error, info, warn};
use postcard::{from_bytes_cobs, to_stdvec_cobs};
use radomctl_protocol::{HostMessage, PositionTarget, RadomMessage};
use std::{env, io, str, time::Duration};
use tokio::time::sleep;
use tokio::{
self,
io::{AsyncBufReadExt, AsyncWriteExt, BufStream},
@ -7,46 +13,82 @@ use tokio::{
sync::{self, mpsc, watch},
time,
};
use tokio_serial::SerialPortBuilderExt;
use tokio_util::codec::{Decoder, Encoder};
use crate::rotctlprotocol::{parse_command, Command};
struct ProtocolCodec;
impl Decoder for ProtocolCodec {
type Item = RadomMessage;
type Error = io::Error;
fn decode(&mut self, src: &mut BytesMut) -> Result<Option<Self::Item>, Self::Error> {
let frame_end = src.as_ref().iter().position(|b| *b == 0);
if let Some(n) = frame_end {
let mut frame = src.split_to(n + 1);
let host_msg = from_bytes_cobs::<RadomMessage>(&mut frame).unwrap();
return Ok(Some(host_msg));
}
Ok(None)
}
}
impl Encoder<HostMessage> for ProtocolCodec {
type Error = io::Error;
fn encode(&mut self, item: HostMessage, dst: &mut BytesMut) -> Result<(), Self::Error> {
let msg_bytes = to_stdvec_cobs(&item).unwrap();
dst.put(msg_bytes.as_slice());
dst.put_u8(0);
Ok(())
}
}
pub async fn control_rotor(
mut rx_cmd: mpsc::Receiver<Command>,
pos_tx: watch::Sender<(f32, f32)>,
radom_port: String,
) -> Result<()> {
let mut actual_az = 0.0;
let mut actual_el = 0.0;
let port = tokio_serial::new(radom_port, 115_200)
.timeout(Duration::from_millis(10))
.open_native_async()
.expect("Failed to open port");
let mut target_az = 0.0;
let mut target_el = 0.0;
let (mut port_writer, mut port_reader) = ProtocolCodec.framed(port).split();
loop {
tokio::select! {
Some(command) = rx_cmd.recv() => {
match command {
Command::SetPos(az, el) => {
info!("Received set pos {} {}", az, el);
target_az = az;
target_el = el;
//info!("Received set pos {} {}", az, el);
port_writer.send(HostMessage::SetTarget(PositionTarget { az, el })).await?;
}
_ => {}
}
},
_ = time::sleep(time::Duration::from_millis(100)) => {
if target_az < actual_az {
actual_az -= 1.0;
} else if target_az > actual_az {
actual_az += 1.0;
}
if target_el < actual_el {
actual_el -= 1.0;
} else if target_el > actual_el {
actual_el += 1.0;
}
pos_tx.send((actual_az, actual_el)).unwrap();
//info!("Requesting status");
port_writer.send(HostMessage::RequestStatus).await?;
},
msg = port_reader.next() => {
match msg {
Some(Ok(msg)) => {
match msg {
RadomMessage::Status(status) => {
//info!("Received status {:?}", status);
pos_tx.send((status.position.az, status.position.el)).unwrap();
}
_ => {}
}
}
_ => {}
}
}
else => return Ok(())
};
}