cu_crsf/
lib.rs

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

cu_crsf/
lib.rs
