Now that the guard is updated disable time compensation again, but keep a guard against invalid state updates

1 files changed, 35 insertions, 31 deletions

diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp
index f03aa9474..46e405526 100644
--- a/src/modules/fw_att_pos_estimator/estimator.cpp
+++ b/src/modules/fw_att_pos_estimator/estimator.cpp
@@ -1678,43 +1678,47 @@ int RecallStates(float statesForFusion[n_states], uint64_t msec)
 {
     int ret = 0;
 
-    int64_t bestTimeDelta = 200;
-    unsigned bestStoreIndex = 0;
-    for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++)
-    {
-        // The time delta can also end up as negative number,
-        // since we might compare future to past or past to future
-        // therefore cast to int64.
-        int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex];
-        if (timeDelta < 0) {
-            timeDelta = -timeDelta;
-        }
-
-        if (timeDelta < bestTimeDelta)
-        {
-            bestStoreIndex = storeIndex;
-            bestTimeDelta = timeDelta;
-        }
-    }
-    if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
-    {
+    // int64_t bestTimeDelta = 200;
+    // unsigned bestStoreIndex = 0;
+    // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++)
+    // {
+    //     // The time delta can also end up as negative number,
+    //     // since we might compare future to past or past to future
+    //     // therefore cast to int64.
+    //     int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex];
+    //     if (timeDelta < 0) {
+    //         timeDelta = -timeDelta;
+    //     }
+
+    //     if (timeDelta < bestTimeDelta)
+    //     {
+    //         bestStoreIndex = storeIndex;
+    //         bestTimeDelta = timeDelta;
+    //     }
+    // }
+    // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
+    // {
+    //     for (uint8_t i=0; i < n_states; i++) {
+    //         if (isfinite(storedStates[i][bestStoreIndex])) {
+    //             statesForFusion[i] = storedStates[i][bestStoreIndex];
+    //         } else if (isfinite(states[i])) {
+    //             statesForFusion[i] = states[i];
+    //         } else {
+    //             // There is not much we can do here, except reporting the error we just
+    //             // found.
+    //             ret++;
+    //         }
+    // }
+    // else // otherwise output current state
+    // {
         for (uint8_t i=0; i < n_states; i++) {
-            if (isfinite(storedStates[i][bestStoreIndex])) {
-                statesForFusion[i] = storedStates[i][bestStoreIndex];
-            } else if (isfinite(states[i])) {
+            if (isfinite(states[i])) {
                 statesForFusion[i] = states[i];
             } else {
-                // There is not much we can do here, except reporting the error we just
-                // found.
                 ret++;
             }
-    }
-    else // otherwise output current state
-    {
-        for (uint8_t i=0; i < n_states; i++) {
-            statesForFusion[i] = states[i];
         }
-    }
+    // }
 
     return ret;
 }