ekf2: tighten terrain validity requirements

- require valid fusion from a range finder or optical flow before
   considering terrain valid again

src/modules/ekf2/EKF/control.cpp

@@ -146,9 +146,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 	// Additional horizontal velocity data from an auxiliary sensor can be fused
 	controlAuxVelFusion(imu_delayed);
 #endif // CONFIG_EKF2_AUXVEL
-	//
+
 #if defined(CONFIG_EKF2_TERRAIN)
 	controlTerrainFakeFusion();
+	updateTerrainValidity();
 #endif // CONFIG_EKF2_TERRAIN
 
 	controlZeroInnovationHeadingUpdate();

src/modules/ekf2/EKF/ekf.h

@@ -99,7 +99,7 @@ public:
 
 #if defined(CONFIG_EKF2_TERRAIN)
 	// terrain estimate
-	bool isTerrainEstimateValid() const;
+	bool isTerrainEstimateValid() const { return _terrain_valid; }
 
 	// get the estimated terrain vertical position relative to the NED origin
 	float getTerrainVertPos() const { return _state.terrain; };
@@ -500,6 +500,8 @@ private:
 #if defined(CONFIG_EKF2_TERRAIN)
 	// Terrain height state estimation
 	float _last_on_ground_posD{0.0f};	///< last vertical position when the in_air status was false (m)
+
+	bool _terrain_valid{false};
 #endif // CONFIG_EKF2_TERRAIN
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
@@ -733,6 +735,8 @@ private:
 	float getTerrainVPos() const { return isTerrainEstimateValid() ? _state.terrain : _last_on_ground_posD; }
 	void controlTerrainFakeFusion();
 
+	void updateTerrainValidity();
+
 # if defined(CONFIG_EKF2_RANGE_FINDER)
 	// update the terrain vertical position estimate using a height above ground measurement from the range finder
 	bool fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, bool update_terrain);

src/modules/ekf2/EKF/terrain_control.cpp

@@ -75,9 +75,31 @@ void Ekf::controlTerrainFakeFusion()
 	}
 }
 
-bool Ekf::isTerrainEstimateValid() const
+void Ekf::updateTerrainValidity()
 {
-	bool valid = false;
+	bool valid_opt_flow_terrain = false;
+	bool valid_rng_terrain = false;
+	bool valid_hagl_var = false;
+
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
+
+	if (_control_status.flags.opt_flow_terrain
+	    && isRecent(_aid_src_optical_flow.time_last_fuse, _params.hgt_fusion_timeout_max)
+	   ) {
+		valid_opt_flow_terrain = true;
+	}
+
+#endif // CONFIG_EKF2_OPTICAL_FLOW
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+
+	if (_control_status.flags.rng_terrain
+	    && isRecent(_aid_src_rng_hgt.time_last_fuse, _params.hgt_fusion_timeout_max)
+	   ) {
+		valid_rng_terrain = true;
+	}
+
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 	if (_time_last_terrain_fuse != 0) {
 		// Assume being valid when the uncertainty is small compared to the height above ground
@@ -85,18 +107,20 @@ bool Ekf::isTerrainEstimateValid() const
 		sym::ComputeHaglInnovVar(P, 0.f, &hagl_var);
 
 		if (hagl_var < fmaxf(sq(0.1f * getHagl()), 0.2f)) {
-			valid = true;
+			valid_hagl_var = true;
 		}
 	}
 
-#if defined(CONFIG_EKF2_RANGE_FINDER)
+	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)
+		   ) {
+			_terrain_valid = true;
+		}
 
-	// Assume that the terrain estimate is always valid when direct observations are fused
-	if (_control_status.flags.rng_terrain && isRecent(_aid_src_rng_hgt.time_last_fuse, _params.hgt_fusion_timeout_max)) {
-		valid = true;
+	} else {
+		// terrain was previously valid, continue considering valid if variance is good
+		_terrain_valid = valid_hagl_var;
 	}
-
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-	return valid;
 }