From cd8f0816117c0414924c1634d4eb583fa661dd44 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 5 Apr 2024 13:30:03 +1100 Subject: [PATCH] AP_NavEKF: correct setting of _filled Co-authored-by: luweiagi --- libraries/AP_NavEKF/EKF_Buffer.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF/EKF_Buffer.cpp b/libraries/AP_NavEKF/EKF_Buffer.cpp index 8474ccecf4255..b557b4e88dd3c 100644 --- a/libraries/AP_NavEKF/EKF_Buffer.cpp +++ b/libraries/AP_NavEKF/EKF_Buffer.cpp @@ -137,7 +137,7 @@ bool ekf_imu_buffer::init(uint32_t size) _size = size; _youngest = 0; _oldest = 0; - _filled = 0; + _filled = false; return true; } @@ -152,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