From 62ab0bf2e75c560aa255ab51aab1f5ebf591ba97 Mon Sep 17 00:00:00 2001 From: chemicstry Date: Tue, 25 Jul 2023 12:07:09 +0300 Subject: [PATCH] stm32/can: implement proper RX timestamps --- embassy-stm32/src/can/bxcan.rs | 39 +++++++++++++++++++++++++-------- examples/stm32f4/src/bin/can.rs | 18 +++++++++++++-- examples/stm32f7/src/bin/can.rs | 4 ++-- tests/stm32/src/bin/can.rs | 13 ++++++++--- 4 files changed, 58 insertions(+), 16 deletions(-) diff --git a/embassy-stm32/src/can/bxcan.rs b/embassy-stm32/src/can/bxcan.rs index 8b8244d4..795becab 100644 --- a/embassy-stm32/src/can/bxcan.rs +++ b/embassy-stm32/src/can/bxcan.rs @@ -16,6 +16,17 @@ use crate::rcc::RccPeripheral; use crate::time::Hertz; use crate::{interrupt, peripherals, Peripheral}; +/// Contains CAN frame and additional metadata. +/// +/// Timestamp is available if `time` feature is enabled. +#[derive(Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub struct Envelope { + #[cfg(feature = "time")] + pub ts: embassy_time::Instant, + pub frame: bxcan::Frame, +} + /// Interrupt handler. pub struct TxInterruptHandler { _phantom: PhantomData, @@ -199,11 +210,11 @@ impl<'d, T: Instance> Can<'d, T> { } /// Returns a tuple of the time the message was received and the message frame - pub async fn read(&mut self) -> Result<(u16, bxcan::Frame), BusError> { + pub async fn read(&mut self) -> Result { poll_fn(|cx| { T::state().err_waker.register(cx.waker()); - if let Poll::Ready((time, frame)) = T::state().rx_queue.recv().poll_unpin(cx) { - return Poll::Ready(Ok((time, frame))); + if let Poll::Ready(envelope) = T::state().rx_queue.recv().poll_unpin(cx) { + return Poll::Ready(Ok(envelope)); } else if let Some(err) = self.curr_error() { return Poll::Ready(Err(err)); } @@ -228,6 +239,10 @@ impl<'d, T: Instance> Can<'d, T> { } unsafe fn receive_fifo(fifo: RxFifo) { + // Generate timestamp as early as possible + #[cfg(feature = "time")] + let ts = embassy_time::Instant::now(); + let state = T::state(); let regs = T::regs(); let fifo_idx = match fifo { @@ -257,15 +272,19 @@ impl<'d, T: Instance> Can<'d, T> { data[0..4].copy_from_slice(&fifo.rdlr().read().0.to_ne_bytes()); data[4..8].copy_from_slice(&fifo.rdhr().read().0.to_ne_bytes()); - let time = fifo.rdtr().read().time(); let frame = Frame::new_data(id, Data::new(&data[0..data_len]).unwrap()); + let envelope = Envelope { + #[cfg(feature = "time")] + ts, + frame, + }; rfr.modify(|v| v.set_rfom(true)); /* NOTE: consensus was reached that if rx_queue is full, packets should be dropped */ - let _ = state.rx_queue.try_send((time, frame)); + let _ = state.rx_queue.try_send(envelope); } } @@ -405,11 +424,11 @@ pub struct CanRx<'c, 'd, T: Instance> { } impl<'c, 'd, T: Instance> CanRx<'c, 'd, T> { - pub async fn read(&mut self) -> Result<(u16, bxcan::Frame), BusError> { + pub async fn read(&mut self) -> Result { poll_fn(|cx| { T::state().err_waker.register(cx.waker()); - if let Poll::Ready((time, frame)) = T::state().rx_queue.recv().poll_unpin(cx) { - return Poll::Ready(Ok((time, frame))); + if let Poll::Ready(envelope) = T::state().rx_queue.recv().poll_unpin(cx) { + return Poll::Ready(Ok(envelope)); } else if let Some(err) = self.curr_error() { return Poll::Ready(Err(err)); } @@ -467,10 +486,12 @@ pub(crate) mod sealed { use embassy_sync::channel::Channel; use embassy_sync::waitqueue::AtomicWaker; + use super::Envelope; + pub struct State { pub tx_waker: AtomicWaker, pub err_waker: AtomicWaker, - pub rx_queue: Channel, + pub rx_queue: Channel, } impl State { diff --git a/examples/stm32f4/src/bin/can.rs b/examples/stm32f4/src/bin/can.rs index f84f74d3..20ce4edc 100644 --- a/examples/stm32f4/src/bin/can.rs +++ b/examples/stm32f4/src/bin/can.rs @@ -10,6 +10,7 @@ use embassy_stm32::can::bxcan::{Fifo, Frame, StandardId}; use embassy_stm32::can::{Can, Rx0InterruptHandler, Rx1InterruptHandler, SceInterruptHandler, TxInterruptHandler}; use embassy_stm32::gpio::{Input, Pull}; use embassy_stm32::peripherals::CAN1; +use embassy_time::Instant; use {defmt_rtt as _, panic_probe as _}; bind_interrupts!(struct Irqs { @@ -51,9 +52,22 @@ async fn main(_spawner: Spawner) { let mut i: u8 = 0; loop { let tx_frame = Frame::new_data(unwrap!(StandardId::new(i as _)), [i]); + let tx_ts = Instant::now(); can.write(&tx_frame).await; - let (_, rx_frame) = can.read().await.unwrap(); - info!("loopback frame {=u8}", unwrap!(rx_frame.data())[0]); + + let envelope = can.read().await.unwrap(); + + // We can measure loopback latency by using receive timestamp in the `Envelope`. + // Our frame is ~55 bits long (exlcuding bit stuffing), so at 1mbps loopback delay is at least 55 us. + // When measured with `tick-hz-1_000_000` actual latency is 80~83 us, giving a combined hardware and software + // overhead of ~25 us. Note that CPU frequency can greatly affect the result. + let latency = envelope.ts.saturating_duration_since(tx_ts); + + info!( + "loopback frame {=u8}, latency: {} us", + unwrap!(envelope.frame.data())[0], + latency.as_micros() + ); i += 1; } } diff --git a/examples/stm32f7/src/bin/can.rs b/examples/stm32f7/src/bin/can.rs index 1b5b377e..e9650f23 100644 --- a/examples/stm32f7/src/bin/can.rs +++ b/examples/stm32f7/src/bin/can.rs @@ -60,7 +60,7 @@ async fn main(spawner: Spawner) { spawner.spawn(send_can_message(tx)).unwrap(); loop { - let frame = rx.read().await.unwrap(); - println!("Received: {:?}", frame); + let envelope = rx.read().await.unwrap(); + println!("Received: {:?}", envelope); } } diff --git a/tests/stm32/src/bin/can.rs b/tests/stm32/src/bin/can.rs index 8bdd3c24..93253ab8 100644 --- a/tests/stm32/src/bin/can.rs +++ b/tests/stm32/src/bin/can.rs @@ -7,6 +7,7 @@ #[path = "../common.rs"] mod common; use common::*; +use defmt::assert; use embassy_executor::Spawner; use embassy_stm32::bind_interrupts; use embassy_stm32::can::bxcan::filter::Mask32; @@ -14,6 +15,7 @@ use embassy_stm32::can::bxcan::{Fifo, Frame, StandardId}; use embassy_stm32::can::{Can, Rx0InterruptHandler, Rx1InterruptHandler, SceInterruptHandler, TxInterruptHandler}; use embassy_stm32::gpio::{Input, Pull}; use embassy_stm32::peripherals::CAN1; +use embassy_time::{Duration, Instant}; use {defmt_rtt as _, panic_probe as _}; bind_interrupts!(struct Irqs { @@ -62,13 +64,18 @@ async fn main(_spawner: Spawner) { let tx_frame = Frame::new_data(unwrap!(StandardId::new(i as _)), [i]); info!("Transmitting frame..."); + let tx_ts = Instant::now(); can.write(&tx_frame).await; info!("Receiving frame..."); - let (time, rx_frame) = can.read().await.unwrap(); + let envelope = can.read().await.unwrap(); - info!("loopback time {}", time); - info!("loopback frame {=u8}", rx_frame.data().unwrap()[0]); + info!("loopback time {}", envelope.ts); + info!("loopback frame {=u8}", envelope.frame.data().unwrap()[0]); + + // Theoretical minimum latency is 55us, actual is usually ~80us + let latency = envelope.ts.saturating_duration_since(tx_ts); + assert!(Duration::from_micros(50) < latency && latency < Duration::from_micros(100)); i += 1; if i > 10 {