Guard against invalid states

2 files changed, 54 insertions, 29 deletions

diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp
index bde46ba03..f03aa9474 100644
--- a/src/modules/fw_att_pos_estimator/estimator.cpp
+++ b/src/modules/fw_att_pos_estimator/estimator.cpp
@@ -1674,34 +1674,49 @@ void ResetStoredStates()
 }
 
 // Output the state vector stored at the time that best matches that specified by msec
-void RecallStates(float statesForFusion[n_states], uint64_t msec)
+int RecallStates(float statesForFusion[n_states], uint64_t msec)
 {
-    // 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];
-    // }
+    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
+    {
+        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++) {
+            statesForFusion[i] = states[i];
+        }
+    }
+
+    return ret;
 }
 
 void quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h
index 55e6eb12e..c5a5e9d8d 100644
--- a/src/modules/fw_att_pos_estimator/estimator.h
+++ b/src/modules/fw_att_pos_estimator/estimator.h
@@ -171,8 +171,18 @@ void quatNorm(float (&quatOut)[4], const float quatIn[4]);
 // store staes along with system time stamp in msces
 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);
+/**
+ * Recall the state vector.
+ *
+ * Recalls the vector stored at closest time to the one specified by msec
+ *
+ * @return zero on success, integer indicating the number of invalid states on failure.
+ *         Does only copy valid states, if the statesForFusion vector was initialized
+ *         correctly by the caller, the result can be safely used, but is a mixture
+ *         time-wise where valid states were updated and invalid remained at the old
+ *         value.
+ */
+int RecallStates(float statesForFusion[n_states], uint64_t msec);
 
 void ResetStoredStates();