cu_msp_src/
lib.rs

1use cu29::prelude::*;
2use cu_msp_lib::structs::MspResponse;
3#[cfg(unix)]
4use cu_msp_lib::MspParser;
5#[cfg(unix)]
6use serialport::SerialPort;
7#[cfg(unix)]
8use serialport::TTYPort;
9use smallvec::SmallVec;
10
11use bincode::de::Decoder;
12use bincode::enc::Encoder;
13use bincode::error::{DecodeError, EncodeError};
14use bincode::{Decode, Encode};
15use serde::{Deserialize, Serialize};
16#[cfg(unix)]
17use std::io::Read;
18
19const MAX_MSG_SIZE: usize = 16;
20
21#[derive(Debug, Clone, Default, Serialize, Deserialize)]
22pub struct MspResponseBatch(pub SmallVec<[MspResponse; MAX_MSG_SIZE]>);
23
24impl Encode for MspResponseBatch {
25    fn encode<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
26        Encode::encode(&self.0.as_slice(), encoder)
27    }
28}
29
30impl Decode<()> for MspResponseBatch {
31    fn decode<D: Decoder<Context = ()>>(decoder: &mut D) -> Result<Self, DecodeError> {
32        // allocations are ok in decode
33        let v = <Vec<MspResponse> as Decode<()>>::decode(decoder)?;
34        Ok(Self(v.into()))
35    }
36}
37
38impl MspResponseBatch {
39    pub fn new() -> Self {
40        Self(SmallVec::new())
41    }
42
43    pub fn push(&mut self, resp: MspResponse) {
44        let MspResponseBatch(ref mut vec) = self;
45        vec.push(resp);
46    }
47
48    pub fn len(&self) -> usize {
49        self.0.len()
50    }
51
52    pub fn is_empty(&self) -> bool {
53        self.0.is_empty()
54    }
55}
56
57pub struct MSPSrc {
58    #[cfg(unix)]
59    serial: TTYPort,
60    #[cfg(unix)]
61    parser: MspParser,
62    buffer: SmallVec<[u8; 512]>,
63}
64
65impl Freezable for MSPSrc {}
66
67impl CuSrcTask for MSPSrc {
68    type Output<'m> = output_msg!(MspResponseBatch);
69
70    fn new(config: Option<&ComponentConfig>) -> CuResult<Self>
71    where
72        Self: Sized,
73    {
74        if config.is_none() {
75            return Err("No config provided".into());
76        }
77        #[cfg(unix)]
78        let port: String = config
79            .and_then(|config| config.get::<String>("device"))
80            .unwrap_or("/dev/ttyUSB0".to_string());
81        #[cfg(unix)]
82        let baudrate = config
83            .and_then(|config| config.get::<u32>("baudrate"))
84            .unwrap_or(115200);
85
86        #[cfg(unix)]
87        let builder = serialport::new(port, baudrate);
88        #[cfg(unix)]
89        let mut serial = TTYPort::open(&builder).unwrap();
90        #[cfg(unix)]
91        serial.set_exclusive(false).unwrap();
92        #[cfg(unix)]
93        serial
94            .set_timeout(std::time::Duration::from_millis(100))
95            .unwrap();
96
97        #[cfg(unix)]
98        let parser = MspParser::new();
99        Ok(Self {
100            #[cfg(unix)]
101            serial,
102            #[cfg(unix)]
103            parser,
104            buffer: SmallVec::new(),
105        })
106    }
107
108    fn process(&mut self, _clock: &RobotClock, output: &mut Self::Output<'_>) -> CuResult<()> {
109        self.buffer.clear();
110        self.buffer.resize(512, 0);
111        #[cfg(unix)]
112        let n = self.serial.read(&mut self.buffer);
113        #[cfg(unix)]
114        if let Err(e) = &n {
115            debug!("read error: {}", e.to_string());
116            return Ok(());
117        }
118        #[cfg(unix)]
119        let n = n.unwrap();
120        #[cfg(unix)]
121        self.buffer.truncate(n);
122
123        #[cfg(unix)]
124        let mut batch = MspResponseBatch::default();
125        #[cfg(unix)]
126        if n > 0 {
127            for &b in &self.buffer {
128                let maybe_packet = self.parser.parse(b);
129                if let Ok(Some(packet)) = maybe_packet {
130                    let response = MspResponse::from(packet);
131                    debug!("Response: {}", &response);
132                    batch.push(response);
133                    if batch.len() >= MAX_MSG_SIZE {
134                        debug!("batch full, sending");
135                        break;
136                    }
137                }
138            }
139        }
140        #[cfg(unix)]
141        output.set_payload(batch);
142
143        #[cfg(windows)]
144        output.set_payload(MspResponseBatch::default());
145        Ok(())
146    }
147}
148
149#[cfg(unix)]
150#[cfg(test)]
151mod tests {
152    use serialport::SerialPort;
153    use serialport::TTYPort;
154    use std::io::Read;
155    use std::io::Write;
156
157    #[test]
158    #[ignore]
159    fn test_concurrent_r_w_serial() {
160        let port = "/dev/ttyS4";
161        let baudrate = 115200;
162        let builder = serialport::new(port, baudrate);
163        let mut r = TTYPort::open(&builder).unwrap();
164        r.set_exclusive(false).unwrap();
165        r.set_timeout(std::time::Duration::from_millis(10)).unwrap();
166
167        let mut w = TTYPort::open(&builder).unwrap();
168        w.set_exclusive(false).unwrap();
169
170        // test loopback
171        let data = [0x01, 0x02, 0x03, 0x04];
172        let nb = w.write(&data).unwrap();
173        assert_eq!(nb, data.len());
174        w.flush().unwrap();
175
176        let mut buf = [0; 4];
177        let nb = r.read(&mut buf).unwrap();
178        assert_eq!(nb, data.len());
179        assert_eq!(data, buf);
180    }
181}
cu_msp_src/lib.rs

cu_msp_src/
lib.rs
