From 8ecec5829264e8743af01c239b336d06417dde98 Mon Sep 17 00:00:00 2001
From: ChristophTobler <christoph.tob@gmail.com>
Date: Mon, 31 Jul 2017 17:59:44 +0200
Subject: [PATCH] Check for stuck range finder measurements in terrain
 estimator

---
 EKF/terrain_estimator.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp
index f21e3cc36a..5d4b5c1fd7 100644
--- a/EKF/terrain_estimator.cpp
+++ b/EKF/terrain_estimator.cpp
@@ -93,7 +93,7 @@ void Ekf::runTerrainEstimator()
 		_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
 
 		// Fuse range finder data if available
-		if (_range_data_ready) {
+		if (_range_data_ready && !_rng_stuck) {
 			fuseHagl();
 
 			// update range sensor angle parameters in case they have changed
@@ -158,7 +158,8 @@ void Ekf::fuseHagl()
 // return true if the terrain estimate is valid
 bool Ekf::get_terrain_valid()
 {
-	if (_terrain_initialised && _range_data_continuous && !_innov_check_fail_status.flags.reject_hagl) {
+	if (_terrain_initialised && _range_data_continuous && !_rng_stuck &&
+		  !_innov_check_fail_status.flags.reject_hagl) {
 		return true;
 
 	} else {