From 2ecad0babb948715be2f525c5e711511ff52a67b Mon Sep 17 00:00:00 2001
From: itscrystalline <pvpthadgaming@gmail.com>
Date: Mon, 16 Feb 2026 22:12:42 +0700
Subject: [PATCH] fix(sensors): add handling edge case where fifo queue size
 and fifo buf size mismatch

---
 sensors/src/gyro.rs | 11 ++++++++++-
 sensors/src/lib.rs  |  1 +
 2 files changed, 11 insertions(+), 1 deletion(-)

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};