From d2945abd8592dd980f5b59cb25422f689d03405a Mon Sep 17 00:00:00 2001
From: ChristophTobler <christoph.tob@gmail.com>
Date: Thu, 13 Jul 2017 14:17:48 +0200
Subject: [PATCH] return hagl valid false if update was rejected

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

diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp
index c3e1ae1d64..a07301fe59 100644
--- a/EKF/terrain_estimator.cpp
+++ b/EKF/terrain_estimator.cpp
@@ -145,6 +145,7 @@ void Ekf::fuseHagl()
 		}
 
 	} else {
+		_innov_check_fail_status.flags.reject_hagl = true;
 		return;
 	}
 }
@@ -155,7 +156,7 @@ bool Ekf::get_terrain_vert_pos(float *ret)
 {
 	memcpy(ret, &_terrain_vpos, sizeof(float));
 
-	if (_terrain_initialised && _range_data_continuous) {
+	if (_terrain_initialised && _range_data_continuous && !_innov_check_fail_status.flags.reject_hagl) {
 		return true;
 
 	} else {