From d8627c6aaf0279efcafd8524ec89e206a7228fd4 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Thu, 25 Feb 2016 12:53:58 +1100
Subject: [PATCH] EKF: Reset the vertical position states if height fusion
 times out

---
 EKF/control.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/EKF/control.cpp b/EKF/control.cpp
index da72eb8eb6..2a4e16c60d 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -96,6 +96,14 @@ void Ekf::controlFusionModes()
 		}
 	}
 
+	// Handle the case where we have rejected height measurements for an extended period
+	// This excessive vibration levels can cause this so a reset gives the filter a chance to recover
+	// After 10 seconds without aiding we reset to the height measurement provided the data is fresh
+	if ((_time_last_imu - _time_last_hgt_fuse > 10e6) && (_time_last_imu - _time_last_baro < 5e5)) {
+		// Reset vertical position and velocity states to the last measurement
+		resetHeight();
+	}
+
 	// handle the case when we are relying on optical flow fusion and lose it
 	if (_control_status.flags.opt_flow && !_control_status.flags.gps) {
 		// TODO