cu_crsf/
lib.rs

1#![cfg_attr(not(feature = "std"), no_std)]
2
3extern crate alloc;
4
5pub mod messages;
6
7use crate::messages::{LinkStatisticsPayload, RcChannelsPayload};
8use alloc::string::String;
9use alloc::vec::Vec;
10use bincode::de::{Decode, Decoder};
11use bincode::enc::{Encode, Encoder};
12use bincode::error::{DecodeError, EncodeError};
13use crsf::{LinkStatistics, Packet, PacketAddress, PacketParser, RcChannels};
14use cu29::cubridge::{
15    BridgeChannel, BridgeChannelConfig, BridgeChannelInfo, BridgeChannelSet, CuBridge,
16};
17use cu29::prelude::*;
18use cu29::resources;
19use embedded_io::{Read, Write};
20
21const READ_BUFFER_SIZE: usize = 1024;
22const PARSER_BUFFER_SIZE: usize = 1024;
23
24fn encode_snapshot_value<T, E>(value: &T, encoder: &mut E) -> Result<(), EncodeError>
25where
26    T: Encode,
27    E: Encoder,
28{
29    let bytes = bincode::encode_to_vec(value, bincode::config::standard())?;
30    Encode::encode(&bytes, encoder)
31}
32
33fn decode_snapshot_value<T, D>(decoder: &mut D) -> Result<T, DecodeError>
34where
35    T: Decode<()>,
36    D: Decoder,
37{
38    let bytes: Vec<u8> = Decode::decode(decoder)?;
39    let (value, bytes_read): (T, usize) =
40        bincode::decode_from_slice(&bytes, bincode::config::standard())?;
41    if bytes_read != bytes.len() {
42        return Err(DecodeError::OtherString(String::from(
43            "CRSF bridge state snapshot had trailing bytes",
44        )));
45    }
46    Ok(value)
47}
48
49rx_channels! {
50    lq_rx => LinkStatisticsPayload,
51    rc_rx => RcChannelsPayload
52    // TODO(gbin): add other types
53}
54
55tx_channels! {
56    lq_tx => LinkStatisticsPayload,
57    rc_tx => RcChannelsPayload
58}
59
60resources!(for<S, E> where S: Write<Error = E> + Read<Error = E> + Send + Sync + 'static {
61    serial => Owned<S>,
62});
63
64/// Crossfire bridge for Copper-rs.
65#[derive(Reflect)]
66#[reflect(from_reflect = false, no_field_bounds, type_path = false)]
67pub struct CrsfBridge<S, E>
68where
69    S: Write<Error = E> + Read<Error = E> + Send + Sync + 'static,
70    E: 'static,
71{
72    #[reflect(ignore)]
73    serial_port: S,
74    #[reflect(ignore)]
75    parser: PacketParser<PARSER_BUFFER_SIZE>,
76    #[reflect(ignore)]
77    last_lq: Option<LinkStatistics>,
78    #[reflect(ignore)]
79    last_rc: Option<RcChannels>,
80}
81
82impl<S, E> CrsfBridge<S, E>
83where
84    S: Write<Error = E> + Read<Error = E> + Send + Sync + 'static,
85    E: 'static,
86{
87    fn from_serial(serial_port: S) -> Self {
88        Self {
89            serial_port,
90            parser: PacketParser::<PARSER_BUFFER_SIZE>::new(),
91            last_lq: None,
92            last_rc: None,
93        }
94    }
95
96    fn write_packet(&mut self, packet: Packet) -> CuResult<()> {
97        let raw_packet = packet.into_raw(PacketAddress::Transmitter);
98        self.serial_port
99            .write_all(raw_packet.data())
100            .map_err(|_| CuError::from("CRSF: Serial port write error"))
101    }
102
103    // decode from the serial buffer and update to the last received values
104    fn update(&mut self, ctx: &CuContext) -> CuResult<()> {
105        let mut buf = [0; READ_BUFFER_SIZE];
106        match self.serial_port.read(buf.as_mut_slice()) {
107            Ok(n) => {
108                if n > 0 {
109                    self.parser.push_bytes(&buf[..n]);
110                    while let Some(Ok((_, packet))) = self.parser.next_packet() {
111                        match packet {
112                            Packet::LinkStatistics(link_statistics) => {
113                                debug!(
114                                    ctx,
115                                    "LinkStatistics: Download LQ:{}",
116                                    link_statistics.downlink_link_quality
117                                );
118                                self.last_lq = Some(link_statistics);
119                            }
120                            Packet::RcChannels(channels) => {
121                                self.last_rc = Some(channels);
122                                for (i, value) in self.last_rc.iter().enumerate() {
123                                    debug!(ctx, "RC Channel {}: {}", i, value.as_ref());
124                                }
125                            }
126                            _ => {
127                                info!(ctx, "CRSF: Received other packet");
128                            }
129                        }
130                    }
131                }
132            }
133            _ => {
134                error!(ctx, "CRSF: Serial port read error");
135            }
136        }
137        Ok(())
138    }
139}
140
141impl<S, E> cu29::reflect::TypePath for CrsfBridge<S, E>
142where
143    S: Write<Error = E> + Read<Error = E> + Send + Sync + 'static,
144    E: 'static,
145{
146    fn type_path() -> &'static str {
147        "cu_crsf::CrsfBridge"
148    }
149
150    fn short_type_path() -> &'static str {
151        "CrsfBridge"
152    }
153
154    fn type_ident() -> Option<&'static str> {
155        Some("CrsfBridge")
156    }
157
158    fn crate_name() -> Option<&'static str> {
159        Some("cu_crsf")
160    }
161
162    fn module_path() -> Option<&'static str> {
163        Some("cu_crsf")
164    }
165}
166
167impl<S, E> Freezable for CrsfBridge<S, E>
168where
169    S: Write<Error = E> + Read<Error = E> + Send + Sync + 'static,
170    E: 'static,
171{
172    fn freeze<Enc: Encoder>(&self, encoder: &mut Enc) -> Result<(), EncodeError> {
173        let last_lq = self.last_lq.clone().map(LinkStatisticsPayload::from);
174        let last_rc = self.last_rc.clone().map(RcChannelsPayload::from);
175        encode_snapshot_value(&last_lq, encoder)?;
176        encode_snapshot_value(&last_rc, encoder)?;
177        Ok(())
178    }
179
180    fn thaw<Dec: Decoder>(&mut self, decoder: &mut Dec) -> Result<(), DecodeError> {
181        let last_lq: Option<LinkStatisticsPayload> = decode_snapshot_value(decoder)?;
182        let last_rc: Option<RcChannelsPayload> = decode_snapshot_value(decoder)?;
183        self.last_lq = last_lq.map(LinkStatistics::from);
184        self.last_rc = last_rc.map(RcChannels::from);
185        Ok(())
186    }
187}
188
189impl<S, E> CuBridge for CrsfBridge<S, E>
190where
191    S: Write<Error = E> + Read<Error = E> + Send + Sync + 'static,
192    E: 'static,
193{
194    type Tx = TxChannels;
195    type Rx = RxChannels;
196    type Resources<'r> = Resources<S, E>;
197
198    fn new(
199        config: Option<&ComponentConfig>,
200        tx_channels: &[BridgeChannelConfig<<Self::Tx as BridgeChannelSet>::Id>],
201        rx_channels: &[BridgeChannelConfig<<Self::Rx as BridgeChannelSet>::Id>],
202        resources: Self::Resources<'_>,
203    ) -> CuResult<Self>
204    where
205        Self: Sized,
206    {
207        let _ = tx_channels;
208        let _ = rx_channels;
209
210        let _ = config;
211
212        Ok(Self::from_serial(resources.serial.0))
213    }
214
215    fn send<'a, Payload>(
216        &mut self,
217        ctx: &CuContext,
218        channel: &'static BridgeChannel<<Self::Tx as BridgeChannelSet>::Id, Payload>,
219        msg: &CuMsg<Payload>,
220    ) -> CuResult<()>
221    where
222        Payload: CuMsgPayload + 'a,
223    {
224        match channel.id() {
225            TxId::LqTx => {
226                let lsi: &CuMsg<LinkStatisticsPayload> = msg.downcast_ref()?;
227                if let Some(lq) = lsi.payload() {
228                    debug!(
229                        ctx,
230                        "CRSF: Sent LinkStatistics: Downlink LQ:{}", lq.0.downlink_link_quality
231                    );
232                    self.write_packet(Packet::LinkStatistics(lq.0.clone()))?;
233                }
234            }
235            TxId::RcTx => {
236                let rccs: &CuMsg<RcChannelsPayload> = msg.downcast_ref()?;
237                if let Some(rc) = rccs.payload() {
238                    for (i, value) in rc.0.iter().enumerate() {
239                        debug!(ctx, "Sending RC Channel {}: {}", i, value);
240                    }
241                    self.write_packet(Packet::RcChannels(rc.0.clone()))?;
242                }
243            }
244        }
245        Ok(())
246    }
247
248    fn receive<'a, Payload>(
249        &mut self,
250        ctx: &CuContext,
251        channel: &'static BridgeChannel<<Self::Rx as BridgeChannelSet>::Id, Payload>,
252        msg: &mut CuMsg<Payload>,
253    ) -> CuResult<()>
254    where
255        Payload: CuMsgPayload + 'a,
256    {
257        self.update(ctx)?;
258        msg.tov = Tov::Time(ctx.now());
259        match channel.id() {
260            RxId::LqRx => {
261                if let Some(lq) = self.last_lq.as_ref() {
262                    let lqp = LinkStatisticsPayload::from(lq.clone());
263                    let lq_msg: &mut CuMsg<LinkStatisticsPayload> = msg.downcast_mut()?;
264                    lq_msg.set_payload(lqp);
265                }
266            }
267            RxId::RcRx => {
268                if let Some(rc) = self.last_rc.as_ref() {
269                    let rc = RcChannelsPayload::from(rc.clone());
270                    let rc_msg: &mut CuMsg<RcChannelsPayload> = msg.downcast_mut()?;
271                    rc_msg.set_payload(rc);
272                }
273            }
274        }
275        Ok(())
276    }
277}
278
279/// Convenience alias for std targets.
280#[cfg(feature = "std")]
281pub type CrsfBridgeStd = CrsfBridge<cu_linux_resources::LinuxSerialPort, std::io::Error>;
cu_crsf/lib.rs

cu_crsf/
lib.rs
