Check for stuck range finder measurements in terrain estimator

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 {