use embassy_stm32::can::{
Can,
enums::BusError,
frame::{FdEnvelope, FdFrame, Frame},
};
use odrive_can_protocol::compat::embassy::FromEmbassyError;
use crate::{AttemptError, BeginSendError, Driver, IngestResult, OperationId};
#[derive(Debug)]
pub enum EmbassyTransmit {
Submitted {
displaced: Option<Frame>,
},
Uncertain {
displaced: Option<Frame>,
error: AttemptError,
},
}
#[derive(Debug)]
pub enum EmbassyReceive {
Frame {
frame: FdFrame,
classification: IngestResult,
},
Invalid {
frame: FdFrame,
error: FromEmbassyError,
},
}
pub async fn transmit(
can: &mut Can<'_>,
driver: &mut Driver,
id: OperationId,
mut now_ms: impl FnMut() -> u64,
) -> Result<EmbassyTransmit, BeginSendError> {
let attempt = driver.begin_send(id, now_ms())?;
let frame = Frame::from(attempt.frame());
let displaced = can.write(&frame).await;
match attempt.submitted(now_ms()) {
Ok(()) => Ok(EmbassyTransmit::Submitted { displaced }),
Err(error) => Ok(EmbassyTransmit::Uncertain { displaced, error }),
}
}
pub async fn receive_with_timestamp(
can: &mut Can<'_>,
driver: &mut Driver,
timestamp_ms: impl FnOnce(&FdEnvelope) -> u64,
) -> Result<EmbassyReceive, BusError> {
let envelope = can.read_fd().await?;
let received_at_ms = timestamp_ms(&envelope);
Ok(ingest_fd_frame(driver, envelope.frame, received_at_ms))
}
pub fn ingest_fd_frame(driver: &mut Driver, frame: FdFrame, received_at_ms: u64) -> EmbassyReceive {
match odrive_can_protocol::FrameRef::try_from(&frame) {
Ok(view) => EmbassyReceive::Frame {
classification: driver.ingest(view, received_at_ms),
frame,
},
Err(error) => EmbassyReceive::Invalid { frame, error },
}
}