Fix for recallstates function

src/modules/fw_att_pos_estimator/estimator.cpp

@@ -1615,34 +1615,34 @@ void StoreStates(uint64_t timestamp_ms)
 }
 
 // Output the state vector stored at the time that best matches that specified by msec
-void RecallStates(float (&statesForFusion)[n_states], uint64_t msec)
+void RecallStates(float statesForFusion[n_states], uint64_t msec)
 {
-    int 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.
-        int 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++) statesForFusion[i] = storedStates[i][bestStoreIndex];
-    }
-    else // otherwise output current state
-    {
+    // 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++) statesForFusion[i] = storedStates[i][bestStoreIndex];
+    // }
+    // else // otherwise output current state
+    // {
         for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = states[i];
-    }
+    // }
 }
 
 void quat2Tnb(Mat3f &Tnb, const float (&quat)[4])

src/modules/fw_att_pos_estimator/estimator.h

@@ -147,7 +147,7 @@ void quatNorm(float (&quatOut)[4], const float quatIn[4]);
 void StoreStates(uint64_t timestamp_ms);
 
 // recall stste vector stored at closest time to the one specified by msec
-void RecallStates(float (&statesForFusion)[n_states], uint64_t msec);
+void RecallStates(float statesForFusion[n_states], uint64_t msec);
 
 void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);