ekf2: track last terrain fuse time and update logic

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp

@@ -99,6 +99,10 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
 		_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
 		_aid_src_optical_flow.fused = true;
 
+		if (update_terrain) {
+			_time_last_terrain_fuse = _time_delayed_us;
+		}
+
 		return true;
 	}
 

src/modules/ekf2/EKF/aid_sources/range_finder/range_height_fusion.cpp

@@ -66,5 +66,9 @@ bool Ekf::fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, boo
 	aid_src.time_last_fuse = _time_delayed_us;
 	aid_src.fused = true;
 
+	if (update_terrain) {
+		_time_last_terrain_fuse = _time_delayed_us;
+	}
+
 	return true;
 }

src/modules/ekf2/EKF/ekf.cpp

@@ -110,6 +110,7 @@ void Ekf::reset()
 	_time_last_hor_vel_fuse = 0;
 	_time_last_ver_vel_fuse = 0;
 	_time_last_heading_fuse = 0;
+	_time_last_terrain_fuse = 0;
 
 	_last_known_pos.setZero();
 

src/modules/ekf2/EKF/ekf.h

@@ -585,6 +585,7 @@ private:
 	uint64_t _time_last_hor_vel_fuse{0};	///< time the last fusion of horizontal velocity measurements was performed (uSec)
 	uint64_t _time_last_ver_vel_fuse{0};	///< time the last fusion of verticalvelocity measurements was performed (uSec)
 	uint64_t _time_last_heading_fuse{0};
+	uint64_t _time_last_terrain_fuse{0};
 
 	Vector3f _last_known_pos{};		///< last known local position vector (m)
 

src/modules/ekf2/EKF/terrain_control.cpp

@@ -67,15 +67,7 @@ void Ekf::controlTerrainFakeFusion()
 	    && !_control_status.flags.rng_terrain
 	    && !_control_status.flags.opt_flow_terrain) {
 
-		bool recent_terrain_aiding = false;
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-		recent_terrain_aiding |= isRecent(_aid_src_rng_hgt.time_last_fuse, (uint64_t)1e6);
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-		recent_terrain_aiding |= isRecent(_aid_src_optical_flow.time_last_fuse, (uint64_t)1e6);
-#endif // CONFIG_EKF2_OPTICAL_FLOW
+		bool recent_terrain_aiding = isRecent(_time_last_terrain_fuse, (uint64_t)1e6);
 
 		if (_control_status.flags.vehicle_at_rest || !recent_terrain_aiding) {
 			initTerrain();
@@ -85,15 +77,22 @@ void Ekf::controlTerrainFakeFusion()
 
 bool Ekf::isTerrainEstimateValid() const
 {
-	// Assume being valid when the uncertainty is small compared to the height above ground
-	float hagl_var = INFINITY;
-	sym::ComputeHaglInnovVar(P, 0.f, &hagl_var);
-	bool valid = hagl_var < fmaxf(sq(0.1f * getHagl()), 0.2f);
+	bool valid = false;
+
+	if (_time_last_terrain_fuse != 0) {
+		// Assume being valid when the uncertainty is small compared to the height above ground
+		float hagl_var = INFINITY;
+		sym::ComputeHaglInnovVar(P, 0.f, &hagl_var);
+
+		if (hagl_var < fmaxf(sq(0.1f * getHagl()), 0.2f)) {
+			valid = true;
+		}
+	}
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 
 	// 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, (uint64_t)5e6)) {
+	if (_control_status.flags.rng_terrain && isRecent(_aid_src_rng_hgt.time_last_fuse, _params.hgt_fusion_timeout_max)) {
 		valid = true;
 	}