diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp
index 0a145a0898..77ec452295 100644
--- a/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp
+++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp
@@ -94,7 +94,7 @@ void CanIface::RxQueue::push(const uavcan::CanFrame& frame, const uint64_t& utc_
         in_ = 0;
     }
     len_++;
-    if (len_ >= capacity_)
+    if (len_ > capacity_)
     {
         len_ = capacity_;
         registerOverflow();