92 lines
2.8 KiB
Rust
92 lines
2.8 KiB
Rust
use anyhow::Result;
|
|
use bytes::{BufMut, BytesMut};
|
|
use futures::{stream::StreamExt, SinkExt};
|
|
use postcard::{from_bytes_cobs, to_stdvec_cobs};
|
|
use radomctl_protocol::{HostMessage, PositionTarget, RadomMessage};
|
|
use std::{io, time::Duration};
|
|
use tokio::{
|
|
self,
|
|
io::AsyncBufReadExt,
|
|
sync::{mpsc, watch},
|
|
time,
|
|
};
|
|
use tokio_serial::SerialPortBuilderExt;
|
|
use tokio_util::codec::{Decoder, Encoder};
|
|
|
|
use crate::rotctlprotocol::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 port = tokio_serial::new(radom_port, 115_200)
|
|
.timeout(Duration::from_millis(10))
|
|
.open_native_async()
|
|
.expect("Failed to open port");
|
|
|
|
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);
|
|
port_writer.send(HostMessage::SetTarget(PositionTarget { az, el })).await?;
|
|
}
|
|
_ => {}
|
|
}
|
|
},
|
|
|
|
_ = time::sleep(time::Duration::from_millis(100)) => {
|
|
//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(())
|
|
};
|
|
}
|
|
}
|