aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF: correct setting of _filled

Browse Source 
Co-authored-by: luweiagi <luweiagi@163.com>
This commit is contained in:
Peter Barker 2024-04-05 13:30:03 +11:00 committed by Peter Barker
parent 99f5e74a8e
commit cd8f081611
1 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_NavEKF/EKF_Buffer.cpp
View File

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