AP_NavEKF: avoid copying all but the best element when recalling time…

…-horizon data

libraries/AP_NavEKF/EKF_Buffer.cpp

@@ -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;
 }