diff --git a/sensors/src/gyro.rs b/sensors/src/gyro.rs index 24fdc3b..6f7dcb7 100644 --- a/sensors/src/gyro.rs +++ b/sensors/src/gyro.rs @@ -245,7 +245,16 @@ pub async fn gyro(i2c: I2C0<'static>, sda: GPIO21<'static>, scl: GPIO22<'static> timeout_ms(gyro.read_fifo(&mut buf), 200).await, "reading fifo", ) { - Ok(b) => b, + Ok(b) if b.len() >= 16 => b, + Ok(b) => { + core::hint::cold_path(); + error!( + "fifo buffer has {} elements instead of {len} elements!", + b.len() + ); + errors += 1; + continue; + } Err(e) => { error!("{e}"); errors += 1; diff --git a/sensors/src/lib.rs b/sensors/src/lib.rs index dc14670..d170bc1 100644 --- a/sensors/src/lib.rs +++ b/sensors/src/lib.rs @@ -1,4 +1,5 @@ #![no_std] +#![feature(cold_path)] use crate::fingers::FlexSensors; use embassy_futures::select::{Either, select};