From 62ab0bf2e75c560aa255ab51aab1f5ebf591ba97 Mon Sep 17 00:00:00 2001 From: chemicstry Date: Tue, 25 Jul 2023 12:07:09 +0300 Subject: [PATCH 1/4] 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 { From a56ef685f3bfd9148a79c9dbbdde83a2c1642ba5 Mon Sep 17 00:00:00 2001 From: chemicstry Date: Tue, 25 Jul 2023 12:19:42 +0300 Subject: [PATCH 2/4] stm32: forward defmt feature to embassy-time --- embassy-stm32/Cargo.toml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/embassy-stm32/Cargo.toml b/embassy-stm32/Cargo.toml index 0fb6fdb5..cb064433 100644 --- a/embassy-stm32/Cargo.toml +++ b/embassy-stm32/Cargo.toml @@ -80,7 +80,7 @@ stm32-metapac = { version = "13", default-features = false, features = ["metadat default = ["rt"] rt = ["stm32-metapac/rt"] -defmt = ["dep:defmt", "bxcan/unstable-defmt", "embassy-sync/defmt", "embassy-embedded-hal/defmt", "embassy-hal-common/defmt", "embedded-io?/defmt", "embassy-usb-driver?/defmt", "embassy-net-driver/defmt"] +defmt = ["dep:defmt", "bxcan/unstable-defmt", "embassy-sync/defmt", "embassy-embedded-hal/defmt", "embassy-hal-common/defmt", "embedded-io?/defmt", "embassy-usb-driver?/defmt", "embassy-net-driver/defmt", "embassy-time?/defmt"] memory-x = ["stm32-metapac/memory-x"] exti = [] @@ -1445,4 +1445,4 @@ stm32wle5cb = [ "stm32-metapac/stm32wle5cb" ] stm32wle5cc = [ "stm32-metapac/stm32wle5cc" ] stm32wle5j8 = [ "stm32-metapac/stm32wle5j8" ] stm32wle5jb = [ "stm32-metapac/stm32wle5jb" ] -stm32wle5jc = [ "stm32-metapac/stm32wle5jc" ] \ No newline at end of file +stm32wle5jc = [ "stm32-metapac/stm32wle5jc" ] From ad85beb6779cf4e970ab91bb0229b9fdf3c2a8ba Mon Sep 17 00:00:00 2001 From: chemicstry Date: Mon, 31 Jul 2023 10:32:17 +0300 Subject: [PATCH 3/4] stm32/can: Add more derives for CAN Envelope --- embassy-stm32/src/can/bxcan.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/embassy-stm32/src/can/bxcan.rs b/embassy-stm32/src/can/bxcan.rs index 448be1cd..a3e3ec86 100644 --- a/embassy-stm32/src/can/bxcan.rs +++ b/embassy-stm32/src/can/bxcan.rs @@ -19,7 +19,7 @@ use crate::{interrupt, peripherals, Peripheral}; /// Contains CAN frame and additional metadata. /// /// Timestamp is available if `time` feature is enabled. -#[derive(Debug)] +#[derive(Debug, Clone, PartialEq, Eq)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] pub struct Envelope { #[cfg(feature = "time")] From 83ab8e057a759ac76c8cddeec1ebdc28c4516b2b Mon Sep 17 00:00:00 2001 From: chemicstry Date: Mon, 31 Jul 2023 13:24:10 +0300 Subject: [PATCH 4/4] stm32/can: Fix latency measurement in tests --- tests/stm32/src/bin/can.rs | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/tests/stm32/src/bin/can.rs b/tests/stm32/src/bin/can.rs index 93253ab8..8737ca8e 100644 --- a/tests/stm32/src/bin/can.rs +++ b/tests/stm32/src/bin/can.rs @@ -67,15 +67,25 @@ async fn main(_spawner: Spawner) { let tx_ts = Instant::now(); can.write(&tx_frame).await; - info!("Receiving frame..."); let envelope = can.read().await.unwrap(); + info!("Frame received!"); 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)); + info!("loopback latency {} us", latency.as_micros()); + + // Theoretical minimum latency is 55us, actual is usually ~80us + const MIN_LATENCY: Duration = Duration::from_micros(50); + const MAX_LATENCY: Duration = Duration::from_micros(150); + assert!( + MIN_LATENCY < latency && latency < MAX_LATENCY, + "{} < {} < {}", + MIN_LATENCY, + latency, + MAX_LATENCY + ); i += 1; if i > 10 {