Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: avoid copying all but the best element when recalling time-horizon data

This commit is contained in:
Peter Barker 2024-07-04 21:00:45 +10:00 committed by Andrew Tridgell
parent 44ae148718
commit 00dc15063e
1 changed files with 7 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF/EKF_Buffer.cpp
View File

@ -53,12 +53,13 @@ uint32_t ekf_ring_buffer::time_ms(uint8_t idx) const
bool ekf_ring_buffer::recall(void *element, const uint32_t sample_time_ms)
{
bool ret = false;
uint8_t best_index = 0; // only valid when ret becomes true
while (count > 0) {
const uint32_t toldest = time_ms(oldest);
const int32_t dt = sample_time_ms - toldest;
const bool matches = dt >= 0 && dt < 100;
if (matches) {
memcpy(element, get_offset(oldest), elsize);
best_index = oldest;
ret = true;
}
if (dt < 0) {
@ -70,6 +71,11 @@ bool ekf_ring_buffer::recall(void *element, const uint32_t sample_time_ms)
count--;
oldest = (oldest+1) % size;
}
if (ret) {
memcpy(element, get_offset(best_index), elsize);
}
return ret;
}