From bce67c6b036cddbe1542f910c6e605256283768f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 18 Mar 2014 18:28:54 +0100
Subject: [PATCH] Init / reinit improvements

---
 .../fw_att_pos_estimator/estimator.cpp        | 41 ++++++++++++++++++-
 src/modules/fw_att_pos_estimator/estimator.h  |  8 ++++
 .../fw_att_pos_estimator_main.cpp             | 21 +++++++++-
 3 files changed, 68 insertions(+), 2 deletions(-)

diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp
index fa81d4dfa0..36b9f4491b 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;
diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h
index b8b218e396..256aff7716 100644
--- a/src/modules/fw_att_pos_estimator/estimator.h
+++ b/src/modules/fw_att_pos_estimator/estimator.h
@@ -158,6 +158,8 @@ 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 ResetStoredStates();
+
 void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
 
 void calcEarthRateNED(Vector3f &omega, float latitude);
@@ -186,5 +188,11 @@ void ConstrainStates();
 
 void ForceSymmetry();
 
+void CheckAndBound();
+
+void ResetPosition();
+
+void ResetVelocity();
+
 uint32_t millis();
 
diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
index df2608f831..8b41e7479d 100644
--- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -604,6 +604,9 @@ FixedwingEstimator::task_main()
 			orb_check(_gps_sub, &gps_updated);
 
 			if (gps_updated) {
+
+				uint64_t last_gps = _gps.timestamp_position;
+
 				orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
 				perf_count(_perf_gps);
 
@@ -612,6 +615,14 @@ FixedwingEstimator::task_main()
 					newDataGps = false;
 
 				} else {
+
+					/* check if we had a GPS outage for a long time */
+					if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
+						ResetPosition();
+						ResetVelocity();
+						ResetStoredStates();
+					}
+
 					/* fuse GPS updates */
 
 					//_gps.timestamp / 1e3;
@@ -687,6 +698,12 @@ FixedwingEstimator::task_main()
 			}
 
 
+			/**
+			 *    CHECK IF THE INPUT DATA IS SANE
+			 */
+			CheckAndBound();
+
+
 			/**
 			 *    PART TWO: EXECUTE THE FILTER
 			 **/
@@ -715,7 +732,9 @@ FixedwingEstimator::task_main()
 					_local_pos.ref_timestamp = _gps.timestamp_position;
 
 					// Store 
-					_baro_ref = baroHgt;
+					orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+					_baro_ref = _baro.altitude;
+					baroHgt = _baro.altitude - _baro_ref;
 					_baro_gps_offset = baroHgt - alt;
 
 					// XXX this is not multithreading safe