cu_crsf/
lib.rs

1#![cfg_attr(not(feature = "std"), no_std)]
2
3extern crate alloc;
4
5pub mod messages;
6
7pub use spin::Mutex;
8
9// std implementation
10#[cfg(feature = "std")]
11mod std_impl {
12    pub const SERIAL_PATH_KEY: &str = "serial_path";
13    pub const SERIAL_BAUD_KEY: &str = "baudrate";
14    pub const SERIAL_TIMEOUT_KEY: &str = "timeout_ms";
15}
16
17#[cfg(feature = "std")]
18use std_impl::*;
19
20use crate::messages::{LinkStatisticsPayload, RcChannelsPayload};
21use crsf::{LinkStatistics, Packet, PacketAddress, PacketParser, RcChannels};
22use cu29::cubridge::{
23    BridgeChannel, BridgeChannelConfig, BridgeChannelInfo, BridgeChannelSet, CuBridge,
24};
25use cu29::prelude::*;
26#[cfg(feature = "std")]
27use cu29::resource::ResourceBundle;
28use cu29::resources;
29use embedded_io::{ErrorType, Read, Write};
30
31const READ_BUFFER_SIZE: usize = 1024;
32const PARSER_BUFFER_SIZE: usize = 1024;
33
34rx_channels! {
35    lq_rx => LinkStatisticsPayload,
36    rc_rx => RcChannelsPayload
37    // TODO(gbin): add other types
38}
39
40tx_channels! {
41    lq_tx => LinkStatisticsPayload,
42    rc_tx => RcChannelsPayload
43}
44
45resources!(for<S, E> where S: Write<Error = E> + Read<Error = E> + Send + Sync + 'static {
46    serial => Owned<S>,
47});
48
49/// Crossfire bridge for Copper-rs.
50pub struct CrsfBridge<S, E>
51where
52    S: Write<Error = E> + Read<Error = E>,
53{
54    serial_port: S,
55    parser: PacketParser<PARSER_BUFFER_SIZE>,
56    last_lq: Option<LinkStatistics>,
57    last_rc: Option<RcChannels>,
58}
59
60impl<S, E> CrsfBridge<S, E>
61where
62    S: Write<Error = E> + Read<Error = E>,
63{
64    fn from_serial(serial_port: S) -> Self {
65        Self {
66            serial_port,
67            parser: PacketParser::<PARSER_BUFFER_SIZE>::new(),
68            last_lq: None,
69            last_rc: None,
70        }
71    }
72
73    // decode from the serial buffer and update to the last received values
74    fn update(&mut self) -> CuResult<()> {
75        let mut buf = [0; READ_BUFFER_SIZE];
76        match self.serial_port.read(buf.as_mut_slice()) {
77            Ok(n) => {
78                if n > 0 {
79                    self.parser.push_bytes(&buf[..n]);
80                    while let Some(Ok((_, packet))) = self.parser.next_packet() {
81                        match packet {
82                            Packet::LinkStatistics(link_statistics) => {
83                                debug!(
84                                    "LinkStatistics: Download LQ:{}",
85                                    link_statistics.downlink_link_quality
86                                );
87                                self.last_lq = Some(link_statistics);
88                            }
89                            Packet::RcChannels(channels) => {
90                                self.last_rc = Some(channels);
91                                for (i, value) in self.last_rc.iter().enumerate() {
92                                    debug!("RC Channel {}: {}", i, value.as_ref());
93                                }
94                            }
95                            _ => {
96                                info!("CRSF: Received other packet");
97                            }
98                        }
99                    }
100                }
101            }
102            _ => {
103                error!("CRSF: Serial port read error");
104            }
105        }
106        Ok(())
107    }
108}
109
110impl<S, E> Freezable for CrsfBridge<S, E> where S: Write<Error = E> + Read<Error = E> {}
111
112impl<S, E> CuBridge for CrsfBridge<S, E>
113where
114    S: Write<Error = E> + Read<Error = E> + Send + Sync + 'static,
115{
116    type Tx = TxChannels;
117    type Rx = RxChannels;
118    type Resources<'r> = Resources<S, E>;
119
120    fn new(
121        config: Option<&ComponentConfig>,
122        tx_channels: &[BridgeChannelConfig<<Self::Tx as BridgeChannelSet>::Id>],
123        rx_channels: &[BridgeChannelConfig<<Self::Rx as BridgeChannelSet>::Id>],
124        resources: Self::Resources<'_>,
125    ) -> CuResult<Self>
126    where
127        Self: Sized,
128    {
129        let _ = tx_channels;
130        let _ = rx_channels;
131
132        let _ = config;
133
134        Ok(Self::from_serial(resources.serial.0))
135    }
136
137    fn send<'a, Payload>(
138        &mut self,
139        _clock: &RobotClock,
140        channel: &'static BridgeChannel<<Self::Tx as BridgeChannelSet>::Id, Payload>,
141        msg: &CuMsg<Payload>,
142    ) -> CuResult<()>
143    where
144        Payload: CuMsgPayload + 'a,
145    {
146        match channel.id() {
147            TxId::LqTx => {
148                let lsi: &CuMsg<LinkStatisticsPayload> = msg.downcast_ref()?;
149                if let Some(lq) = lsi.payload() {
150                    debug!(
151                        "CRSF: Sent LinkStatistics: Downlink LQ:{}",
152                        lq.0.downlink_link_quality
153                    );
154                    let ls = Packet::LinkStatistics(lq.0.clone());
155                    let raw_packet = ls.into_raw(PacketAddress::Transmitter);
156                    self.serial_port
157                        .write_all(raw_packet.data())
158                        .map_err(|_| CuError::from("CRSF: Serial port write error"))?;
159                }
160            }
161            TxId::RcTx => {
162                let rccs: &CuMsg<RcChannelsPayload> = msg.downcast_ref()?;
163                if let Some(rc) = rccs.payload() {
164                    for (i, value) in rc.0.iter().enumerate() {
165                        debug!("Sending RC Channel {}: {}", i, value);
166                    }
167                    let rc = Packet::RcChannels(rc.0.clone());
168                    let raw_packet = rc.into_raw(PacketAddress::Transmitter);
169                    self.serial_port
170                        .write_all(raw_packet.data())
171                        .map_err(|_| CuError::from("CRSF: Serial port write error"))?;
172                }
173            }
174        }
175        Ok(())
176    }
177
178    fn receive<'a, Payload>(
179        &mut self,
180        clock: &RobotClock,
181        channel: &'static BridgeChannel<<Self::Rx as BridgeChannelSet>::Id, Payload>,
182        msg: &mut CuMsg<Payload>,
183    ) -> CuResult<()>
184    where
185        Payload: CuMsgPayload + 'a,
186    {
187        self.update()?;
188        msg.tov = Tov::Time(clock.now());
189        match channel.id() {
190            RxId::LqRx => {
191                if let Some(lq) = self.last_lq.as_ref() {
192                    let lqp = LinkStatisticsPayload::from(lq.clone());
193                    let lq_msg: &mut CuMsg<LinkStatisticsPayload> = msg.downcast_mut()?;
194                    lq_msg.set_payload(lqp);
195                }
196            }
197            RxId::RcRx => {
198                if let Some(rc) = self.last_rc.as_ref() {
199                    let rc = RcChannelsPayload::from(rc.clone());
200                    let rc_msg: &mut CuMsg<RcChannelsPayload> = msg.downcast_mut()?;
201                    rc_msg.set_payload(rc);
202                }
203            }
204        }
205        Ok(())
206    }
207}
208
209#[cfg(feature = "std")]
210pub mod std_serial {
211    use embedded_io_adapters::std::FromStd;
212    use std::boxed::Box;
213    use std::time::Duration;
214
215    pub type StdSerial = FromStd<Box<dyn serialport::SerialPort>>;
216
217    pub fn open(path: &str, baud: u32, timeout_ms: u64) -> std::io::Result<StdSerial> {
218        let port = serialport::new(path, baud)
219            .timeout(Duration::from_millis(timeout_ms))
220            .open()?;
221        Ok(FromStd::new(port))
222    }
223}
224
225#[cfg(feature = "std")]
226pub struct StdSerialBundle;
227
228#[cfg(feature = "std")]
229bundle_resources!(StdSerialBundle: Serial);
230
231#[cfg(feature = "std")]
232impl ResourceBundle for StdSerialBundle {
233    fn build(
234        bundle: cu29::resource::BundleContext<Self>,
235        config: Option<&ComponentConfig>,
236        manager: &mut ResourceManager,
237    ) -> CuResult<()> {
238        let cfg = config.ok_or_else(|| {
239            CuError::from(format!(
240                "CRSF serial bundle `{}` requires configuration",
241                bundle.bundle_id()
242            ))
243        })?;
244        let path = cfg.get::<String>(SERIAL_PATH_KEY).ok_or_else(|| {
245            CuError::from(format!(
246                "CRSF serial bundle `{}` missing `serial_path` entry",
247                bundle.bundle_id()
248            ))
249        })?;
250        let baud = cfg.get::<u32>(SERIAL_BAUD_KEY).unwrap_or(420_000);
251        let timeout_ms = cfg.get::<u32>(SERIAL_TIMEOUT_KEY).unwrap_or(100) as u64;
252
253        let serial = std_serial::open(&path, baud, timeout_ms).map_err(|err| {
254            CuError::from(format!(
255                "Failed to open serial `{path}` at {baud} baud: {err}"
256            ))
257        })?;
258        let key = bundle.key(StdSerialBundleId::Serial);
259        manager.add_owned(key, LockedSerial::new(serial))
260    }
261}
262
263pub struct LockedSerial<T>(pub Mutex<T>);
264
265impl<T> LockedSerial<T> {
266    pub fn new(inner: T) -> Self {
267        Self(Mutex::new(inner))
268    }
269}
270
271impl<T: ErrorType> ErrorType for LockedSerial<T> {
272    type Error = T::Error;
273}
274
275impl<T: Read> Read for LockedSerial<T> {
276    fn read(&mut self, buf: &mut [u8]) -> Result<usize, Self::Error> {
277        let mut guard = self.0.lock();
278        guard.read(buf)
279    }
280}
281
282impl<T: Write> Write for LockedSerial<T> {
283    fn write(&mut self, buf: &[u8]) -> Result<usize, Self::Error> {
284        let mut guard = self.0.lock();
285        guard.write(buf)
286    }
287
288    fn flush(&mut self) -> Result<(), Self::Error> {
289        let mut guard = self.0.lock();
290        guard.flush()
291    }
292}
293
294/// Convenience alias for std targets.
295#[cfg(feature = "std")]
296pub type CrsfBridgeStd = CrsfBridge<LockedSerial<std_serial::StdSerial>, std::io::Error>;
cu_crsf/lib.rs

cu_crsf/
lib.rs
