From da67b2b241ca8eff0d25bfe10b0213f3d570dd50 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 10 May 2014 17:17:25 +0200
Subject: [PATCH] Disable time compensation for further testing

---
 .../ekf_att_pos_estimator/estimator.cpp       | 66 +++++++++----------
 1 file changed, 33 insertions(+), 33 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp
index a6d6a9db55..5a6f879b8c 100644
--- a/src/modules/ekf_att_pos_estimator/estimator.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator.cpp
@@ -1815,41 +1815,41 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec)
 {
     int ret = 0;
 
-    int64_t bestTimeDelta = 200;
-    unsigned bestStoreIndex = 0;
-    for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++)
-    {
-        // Work around a GCC compiler bug - we know 64bit support on ARM is
-        // sketchy in GCC.
-        uint64_t timeDelta;
+    // int64_t bestTimeDelta = 200;
+    // unsigned bestStoreIndex = 0;
+    // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++)
+    // {
+    //     // Work around a GCC compiler bug - we know 64bit support on ARM is
+    //     // sketchy in GCC.
+    //     uint64_t timeDelta;
 
-        if (msec > statetimeStamp[storeIndex]) {
-            timeDelta = msec - statetimeStamp[storeIndex];
-        } else {
-            timeDelta = statetimeStamp[storeIndex] - msec;
-        }
+    //     if (msec > statetimeStamp[storeIndex]) {
+    //         timeDelta = msec - statetimeStamp[storeIndex];
+    //     } else {
+    //         timeDelta = statetimeStamp[storeIndex] - msec;
+    //     }
 
-        if (timeDelta < bestTimeDelta)
-        {
-            bestStoreIndex = storeIndex;
-            bestTimeDelta = timeDelta;
-        }
-    }
-    if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
-    {
-        for (unsigned 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
+    //     if (timeDelta < bestTimeDelta)
+    //     {
+    //         bestStoreIndex = storeIndex;
+    //         bestTimeDelta = timeDelta;
+    //     }
+    // }
+    // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
+    // {
+    //     for (unsigned 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 (unsigned i = 0; i < n_states; i++) {
             if (isfinite(states[i])) {