Init / reinit improvements

1 files changed, 40 insertions, 1 deletions

diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp
index fa81d4dfa..36b9f4491 100644
--- a/src/modules/fw_att_pos_estimator/estimator.cpp
+++ b/src/modules/fw_att_pos_estimator/estimator.cpp
@@ -1651,7 +1651,7 @@ void StoreStates(uint64_t timestamp_ms)
         storeIndex = 0;
 }
 
-void ResetStates()
+void ResetStoredStates()
 {
     // reset all stored states
     memset(&storedStates[0][0], 0, sizeof(storedStates));
@@ -2032,6 +2032,45 @@ void ResetVelocity(void)
     }
 }
 
+
+/**
+ * Check the filter inputs and bound its operational state
+ *
+ * This check will reset the filter states if required
+ * due to a failure of consistency or timeout checks.
+ * it should be run after the measurement data has been
+ * updated, but before any of the fusion steps are
+ * executed.
+ */
+void CheckAndBound()
+{
+
+    // Store the old filter state
+    bool currStaticMode = staticMode;
+
+    // Reset the filter if the IMU data is too old
+    if (dtIMU > 0.2f) {
+        ResetVelocity();
+        ResetPosition();
+        ResetHeight();
+        ResetStoredStates();
+
+        // that's all we can do here, return
+        return;
+    }
+
+    // Check if we're on ground - this also sets static mode.
+    OnGroundCheck();
+
+    // Check if we switched between states
+    if (currStaticMode != staticMode) {
+        ResetVelocity();
+        ResetPosition();
+        ResetHeight();
+        ResetStoredStates();
+    }
+}
+
 void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat)
 {
     float initialRoll, initialPitch;