#![cfg_attr(not(feature = "std"), no_std)]
extern crate alloc;
pub mod messages;
use crate::messages::{LinkStatisticsPayload, RcChannelsPayload};
use alloc::string::String;
use alloc::vec::Vec;
use bincode::de::{Decode, Decoder};
use bincode::enc::{Encode, Encoder};
use bincode::error::{DecodeError, EncodeError};
use crsf::{LinkStatistics, Packet, PacketAddress, PacketParser, RcChannels};
use cu29::cubridge::{
BridgeChannel, BridgeChannelConfig, BridgeChannelInfo, BridgeChannelSet, CuBridge,
};
use cu29::prelude::*;
use cu29::resources;
use embedded_io::{Read, Write};
const READ_BUFFER_SIZE: usize = 1024;
const PARSER_BUFFER_SIZE: usize = 1024;
fn encode_snapshot_value<T, E>(value: &T, encoder: &mut E) -> Result<(), EncodeError>
where
T: Encode,
E: Encoder,
{
let bytes = bincode::encode_to_vec(value, bincode::config::standard())?;
Encode::encode(&bytes, encoder)
}
fn decode_snapshot_value<T, D>(decoder: &mut D) -> Result<T, DecodeError>
where
T: Decode<()>,
D: Decoder,
{
let bytes: Vec<u8> = Decode::decode(decoder)?;
let (value, bytes_read): (T, usize) =
bincode::decode_from_slice(&bytes, bincode::config::standard())?;
if bytes_read != bytes.len() {
return Err(DecodeError::OtherString(String::from(
"CRSF bridge state snapshot had trailing bytes",
)));
}
Ok(value)
}
rx_channels! {
lq_rx => LinkStatisticsPayload,
rc_rx => RcChannelsPayload
}
tx_channels! {
lq_tx => LinkStatisticsPayload,
rc_tx => RcChannelsPayload
}
resources!(for<S, E> where S: Write<Error = E> + Read<Error = E> + Send + Sync + 'static {
serial => Owned<S>,
});
#[derive(Reflect)]
#[reflect(from_reflect = false, no_field_bounds, type_path = false)]
pub struct CrsfBridge<S, E>
where
S: Write<Error = E> + Read<Error = E> + Send + Sync + 'static,
E: 'static,
{
#[reflect(ignore)]
serial_port: S,
#[reflect(ignore)]
parser: PacketParser<PARSER_BUFFER_SIZE>,
#[reflect(ignore)]
last_lq: Option<LinkStatistics>,
#[reflect(ignore)]
last_rc: Option<RcChannels>,
}
impl<S, E> CrsfBridge<S, E>
where
S: Write<Error = E> + Read<Error = E> + Send + Sync + 'static,
E: 'static,
{
fn from_serial(serial_port: S) -> Self {
Self {
serial_port,
parser: PacketParser::<PARSER_BUFFER_SIZE>::new(),
last_lq: None,
last_rc: None,
}
}
fn write_packet(&mut self, packet: Packet) -> CuResult<()> {
let raw_packet = packet.into_raw(PacketAddress::Transmitter);
self.serial_port
.write_all(raw_packet.data())
.map_err(|_| CuError::from("CRSF: Serial port write error"))
}
fn update(&mut self, ctx: &CuContext) -> CuResult<()> {
let mut buf = [0; READ_BUFFER_SIZE];
match self.serial_port.read(buf.as_mut_slice()) {
Ok(n) => {
if n > 0 {
self.parser.push_bytes(&buf[..n]);
while let Some(Ok((_, packet))) = self.parser.next_packet() {
match packet {
Packet::LinkStatistics(link_statistics) => {
debug!(
ctx,
"LinkStatistics: Download LQ:{}",
link_statistics.downlink_link_quality
);
self.last_lq = Some(link_statistics);
}
Packet::RcChannels(channels) => {
self.last_rc = Some(channels);
for (i, value) in self.last_rc.iter().enumerate() {
debug!(ctx, "RC Channel {}: {}", i, value.as_ref());
}
}
_ => {
info!(ctx, "CRSF: Received other packet");
}
}
}
}
}
_ => {
error!(ctx, "CRSF: Serial port read error");
}
}
Ok(())
}
}
impl<S, E> cu29::reflect::TypePath for CrsfBridge<S, E>
where
S: Write<Error = E> + Read<Error = E> + Send + Sync + 'static,
E: 'static,
{
fn type_path() -> &'static str {
"cu_crsf::CrsfBridge"
}
fn short_type_path() -> &'static str {
"CrsfBridge"
}
fn type_ident() -> Option<&'static str> {
Some("CrsfBridge")
}
fn crate_name() -> Option<&'static str> {
Some("cu_crsf")
}
fn module_path() -> Option<&'static str> {
Some("cu_crsf")
}
}
impl<S, E> Freezable for CrsfBridge<S, E>
where
S: Write<Error = E> + Read<Error = E> + Send + Sync + 'static,
E: 'static,
{
fn freeze<Enc: Encoder>(&self, encoder: &mut Enc) -> Result<(), EncodeError> {
let last_lq = self.last_lq.clone().map(LinkStatisticsPayload::from);
let last_rc = self.last_rc.clone().map(RcChannelsPayload::from);
encode_snapshot_value(&last_lq, encoder)?;
encode_snapshot_value(&last_rc, encoder)?;
Ok(())
}
fn thaw<Dec: Decoder>(&mut self, decoder: &mut Dec) -> Result<(), DecodeError> {
let last_lq: Option<LinkStatisticsPayload> = decode_snapshot_value(decoder)?;
let last_rc: Option<RcChannelsPayload> = decode_snapshot_value(decoder)?;
self.last_lq = last_lq.map(LinkStatistics::from);
self.last_rc = last_rc.map(RcChannels::from);
Ok(())
}
}
impl<S, E> CuBridge for CrsfBridge<S, E>
where
S: Write<Error = E> + Read<Error = E> + Send + Sync + 'static,
E: 'static,
{
type Tx = TxChannels;
type Rx = RxChannels;
type Resources<'r> = Resources<S, E>;
fn new(
config: Option<&ComponentConfig>,
tx_channels: &[BridgeChannelConfig<<Self::Tx as BridgeChannelSet>::Id>],
rx_channels: &[BridgeChannelConfig<<Self::Rx as BridgeChannelSet>::Id>],
resources: Self::Resources<'_>,
) -> CuResult<Self>
where
Self: Sized,
{
let _ = tx_channels;
let _ = rx_channels;
let _ = config;
Ok(Self::from_serial(resources.serial.0))
}
fn send<'a, Payload>(
&mut self,
ctx: &CuContext,
channel: &'static BridgeChannel<<Self::Tx as BridgeChannelSet>::Id, Payload>,
msg: &CuMsg<Payload>,
) -> CuResult<()>
where
Payload: CuMsgPayload + 'a,
{
match channel.id() {
TxId::LqTx => {
let lsi: &CuMsg<LinkStatisticsPayload> = msg.downcast_ref()?;
if let Some(lq) = lsi.payload() {
debug!(
ctx,
"CRSF: Sent LinkStatistics: Downlink LQ:{}", lq.0.downlink_link_quality
);
self.write_packet(Packet::LinkStatistics(lq.0.clone()))?;
}
}
TxId::RcTx => {
let rccs: &CuMsg<RcChannelsPayload> = msg.downcast_ref()?;
if let Some(rc) = rccs.payload() {
for (i, value) in rc.0.iter().enumerate() {
debug!(ctx, "Sending RC Channel {}: {}", i, value);
}
self.write_packet(Packet::RcChannels(rc.0.clone()))?;
}
}
}
Ok(())
}
fn receive<'a, Payload>(
&mut self,
ctx: &CuContext,
channel: &'static BridgeChannel<<Self::Rx as BridgeChannelSet>::Id, Payload>,
msg: &mut CuMsg<Payload>,
) -> CuResult<()>
where
Payload: CuMsgPayload + 'a,
{
self.update(ctx)?;
msg.tov = Tov::Time(ctx.now());
match channel.id() {
RxId::LqRx => {
if let Some(lq) = self.last_lq.as_ref() {
let lqp = LinkStatisticsPayload::from(lq.clone());
let lq_msg: &mut CuMsg<LinkStatisticsPayload> = msg.downcast_mut()?;
lq_msg.set_payload(lqp);
}
}
RxId::RcRx => {
if let Some(rc) = self.last_rc.as_ref() {
let rc = RcChannelsPayload::from(rc.clone());
let rc_msg: &mut CuMsg<RcChannelsPayload> = msg.downcast_mut()?;
rc_msg.set_payload(rc);
}
}
}
Ok(())
}
}
#[cfg(feature = "std")]
pub type CrsfBridgeStd = CrsfBridge<cu_linux_resources::LinuxSerialPort, std::io::Error>;