cu_msp_sink/
lib.rs

1use bincode::de::Decoder;
2use bincode::enc::Encoder;
3use bincode::error::{DecodeError, EncodeError};
4use bincode::{Decode, Encode};
5use cu29::prelude::*;
6use cu_msp_lib::structs::MspRequest;
7use cu_msp_lib::MspPacket;
8use serde::{Deserialize, Serialize};
9#[cfg(unix)]
10use serialport::SerialPort;
11#[cfg(unix)]
12use serialport::TTYPort;
13#[cfg(unix)]
14use std::io::Write;
15
16use smallvec::SmallVec;
17
18const MAX_MSG_SIZE: usize = 8;
19
20#[derive(Debug, Clone, Default, Serialize, Deserialize)]
21pub struct MspRequestBatch(pub SmallVec<[MspRequest; MAX_MSG_SIZE]>);
22
23impl MspRequestBatch {
24    pub fn new() -> Self {
25        Self(SmallVec::new())
26    }
27
28    pub fn push(&mut self, req: MspRequest) {
29        let MspRequestBatch(ref mut vec) = self;
30        vec.push(req);
31    }
32}
33
34impl Encode for MspRequestBatch {
35    fn encode<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
36        Encode::encode(&self.0.as_slice(), encoder)
37    }
38}
39
40impl Decode<()> for MspRequestBatch {
41    fn decode<D: Decoder<Context = ()>>(decoder: &mut D) -> Result<Self, DecodeError> {
42        let v = <Vec<MspRequest> as Decode<()>>::decode(decoder)?;
43        Ok(Self(v.into()))
44    }
45}
46
47pub struct MSPSink {
48    #[cfg(unix)]
49    serial: TTYPort,
50}
51
52impl Freezable for MSPSink {}
53
54impl CuSinkTask for MSPSink {
55    type Input<'m> = input_msg!(MspRequestBatch);
56
57    fn new(config: Option<&ComponentConfig>) -> CuResult<Self>
58    where
59        Self: Sized,
60    {
61        if config.is_none() {
62            return Err("No config provided".into());
63        }
64        #[cfg(unix)]
65        let port: String = config
66            .and_then(|config| config.get::<String>("device"))
67            .unwrap_or("/dev/ttyUSB0".to_string());
68        #[cfg(unix)]
69        let baudrate = config
70            .and_then(|config| config.get::<u32>("baudrate"))
71            .unwrap_or(115200);
72
73        #[cfg(unix)]
74        let builder = serialport::new(port, baudrate);
75
76        #[cfg(unix)]
77        let mut serial = TTYPort::open(&builder).unwrap();
78        #[cfg(unix)]
79        serial.set_exclusive(false).unwrap();
80        #[cfg(unix)]
81        serial
82            .set_timeout(std::time::Duration::from_millis(10))
83            .unwrap();
84
85        Ok(Self {
86            #[cfg(unix)]
87            serial,
88        })
89    }
90
91    fn process(&mut self, _clock: &RobotClock, input: &Self::Input<'_>) -> CuResult<()> {
92        if let Some(batch) = input.payload() {
93            let MspRequestBatch(ref batch) = batch;
94            for req in batch {
95                debug!("Sending request {}", req);
96                let msp_packet: MspPacket = req.into();
97                let size = msp_packet.packet_size_bytes();
98                let mut buffer = SmallVec::<[u8; 256]>::new();
99                buffer.resize(size, 0);
100                msp_packet.serialize(&mut buffer).map_err(|e| {
101                    debug!("Error Serializing packet {}", e.to_string());
102                    CuError::new_with_cause("failed to serialize msp packet", e)
103                })?;
104                #[cfg(unix)]
105                self.serial.write_all(buffer.as_slice()).map_err(|e| {
106                    debug!("Error writing to serial port {}", e.to_string());
107                    CuError::new_with_cause("failed to write to serial port", e)
108                })?;
109            }
110        }
111        Ok(())
112    }
113}
cu_msp_sink/lib.rs

cu_msp_sink/
lib.rs
