Correct off-by-one problem in EKF IMU ringbuffer

libraries/AP_NavEKF/EKF_Buffer.cpp

@@ -137,6 +137,7 @@ bool ekf_imu_buffer::init(uint32_t size)
     _size = size;
     _youngest = 0;
     _oldest = 0;
+    _filled = false;
     return true;
 }
 
@@ -151,13 +152,14 @@ void ekf_imu_buffer::push_youngest_element(const void *element)
         return;
     }
     // push youngest to the buffer
-    _youngest = (_youngest+1) % _size;
+    _youngest++;
+    if (_youngest == _size) {
+        _youngest = 0;
+        _filled = true;
+    }
     memcpy(get_offset(_youngest), element, elsize);
     // set oldest data index
     _oldest = (_youngest+1) % _size;
-    if (_oldest == 0) {
-        _filled = true;
-    }
 }
 
 // retrieve the oldest data from the ring buffer tail

libraries/AP_NavEKF/tests/test_ring_buffer.cpp

@@ -7,6 +7,9 @@
 #include <AP_NavEKF/EKF_Buffer.h>
 #include <stdlib.h>
 
+#include <AP_HAL/AP_HAL.h>
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
 
 TEST(EKF_Buffer, EKF_Buffer)
@@ -119,6 +122,60 @@ TEST(EKF_Buffer, EKF_Buffer)
     EXPECT_FALSE(buf.recall(d2, 103));
 }
 
+TEST(ekf_imu_buffer, one_element_case)
+{
+    // test degenerate 1-element case:
+    struct element {
+        uint8_t value;
+    };
+    ekf_imu_buffer *b = new ekf_imu_buffer(sizeof(element));
+    b->init(1);  // 1 element
+    EXPECT_EQ(b->is_filled(), false);
+    EXPECT_EQ(b->get_oldest_index(), b->get_youngest_index());
+    const element e { 34 };
+    b->push_youngest_element((void*)&e);
+    EXPECT_EQ(b->is_filled(), true);
+    EXPECT_EQ(b->get_oldest_index(), b->get_youngest_index());
+    element returned_element {};
+    b->get_oldest_element((void*)&returned_element);
+    EXPECT_EQ(e.value, returned_element.value);
+    element *another_returned_element = (element*)b->get(0);
+    EXPECT_EQ(e.value, another_returned_element->value);
+    // we don't do bounds checking, so get here returns non-nullptr:
+    // another_returned_element = (element*)b.get(17);
+    // EXPECT_EQ(another_returned_element, nullptr);
+    b->reset();
+    EXPECT_EQ(b->get_oldest_index(), b->get_youngest_index());
+    // we don't do bounds checking, so get here returns non-nullptr:
+    // another_returned_element = (element*)b.get(0);
+    // EXPECT_EQ(another_returned_element, nullptr);
+}
+
+TEST(ekf_imu_buffer, is_filled)
+{
+    // https://github.com/ArduPilot/ardupilot/issues/25316
+    struct element {
+        uint8_t value;
+    };
+    ekf_imu_buffer *b = new ekf_imu_buffer(sizeof(element));
+    b->init(4);  // 4 elements
+    const element e { 34 };
+
+    EXPECT_EQ(b->is_filled(), false);
+
+    b->push_youngest_element((void*)&e);
+    EXPECT_EQ(b->is_filled(), false);
+
+    b->push_youngest_element((void*)&e);
+    EXPECT_EQ(b->is_filled(), false);
+
+    b->push_youngest_element((void*)&e);
+    EXPECT_EQ(b->is_filled(), false);
+
+    b->push_youngest_element((void*)&e);
+    EXPECT_EQ(b->is_filled(), true);
+}
+
 AP_GTEST_MAIN()
 
 #endif // HAL_SITL or HAL_LINUX