ekf2-terrain: do not consider negative hagl as valid

src/modules/ekf2/EKF/terrain_control.cpp

@@ -79,7 +79,8 @@ void Ekf::updateTerrainValidity()
 {
 	bool valid_opt_flow_terrain = false;
 	bool valid_rng_terrain = false;
-	bool valid_hagl_var = false;
+	bool positive_hagl_var = false;
+	bool small_relative_hagl_var = false;
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 
@@ -106,21 +107,29 @@ void Ekf::updateTerrainValidity()
 		float hagl_var = INFINITY;
 		sym::ComputeHaglInnovVar(P, 0.f, &hagl_var);
 
-		if (hagl_var < fmaxf(sq(0.1f * getHagl()), 0.2f)) {
-			valid_hagl_var = true;
+		positive_hagl_var = hagl_var > 0.f;
+
+		if (positive_hagl_var
+		    && (hagl_var < sq(fmaxf(0.1f * getHagl(), 0.5f)))
+		   ) {
+			small_relative_hagl_var = true;
 		}
 	}
 
+	const bool positive_hagl = getHagl() >= 0.f;
+
 	if (!_terrain_valid) {
 		// require valid RNG or optical flow (+valid variance) to initially consider terrain valid
-		if (valid_rng_terrain
-		    || (valid_opt_flow_terrain && valid_hagl_var)
+		if (positive_hagl
+		    && positive_hagl_var
+		    && (valid_rng_terrain
+			|| (valid_opt_flow_terrain && small_relative_hagl_var))
 		   ) {
 			_terrain_valid = true;
 		}
 
 	} else {
 		// terrain was previously valid, continue considering valid if variance is good
-		_terrain_valid = valid_hagl_var;
+		_terrain_valid = positive_hagl && positive_hagl_var && small_relative_hagl_var;
 	}
 }