ekf2: cleanup terrain estimator init (shouldn't be valid by default)

- with Ekf::initialiseFilter() resetting _time_last_hagl_fuse the terrain estimate was technically valid regardless of any range or flow data availability and as a result optical flow fusion is able to start and stay active
 - only consider terrain estimate valid based on control status flags and recent fusion

src/modules/ekf2/EKF/ekf.cpp

@@ -204,8 +204,6 @@ bool Ekf::initialiseFilter()
 	_time_last_hgt_fuse = _imu_sample_delayed.time_us;
 	_time_last_hor_pos_fuse = _imu_sample_delayed.time_us;
 	_time_last_hor_vel_fuse = _imu_sample_delayed.time_us;
-	_time_last_hagl_fuse = _imu_sample_delayed.time_us;
-	_time_last_flow_terrain_fuse = _imu_sample_delayed.time_us;
 
 	// reset the output predictor state history to match the EKF initial values
 	alignOutputFilter();

src/modules/ekf2/EKF/ekf.h

@@ -285,7 +285,7 @@ public:
 		return !_vertical_velocity_deadreckon_time_exceeded && !_control_status.flags.fake_hgt;
 	}
 
-	bool isTerrainEstimateValid() const { return _hagl_valid; };
+	bool isTerrainEstimateValid() const;
 
 	bool isYawFinalAlignComplete() const
 	{
@@ -451,8 +451,6 @@ private:
 	void updateHorizontalDeadReckoningstatus();
 	void updateVerticalDeadReckoningStatus();
 
-	void updateTerrainValidity();
-
 	struct {
 		uint8_t velNE_counter;	///< number of horizontal position reset events (allow to wrap if count exceeds 255)
 		uint8_t velD_counter;	///< number of vertical velocity reset events (allow to wrap if count exceeds 255)
@@ -630,7 +628,6 @@ private:
 	float _terrain_var{1e4f};		///< variance of terrain position estimate (m**2)
 	uint8_t _terrain_vpos_reset_counter{0};	///< number of times _terrain_vpos has been reset
 	uint64_t _time_last_hagl_fuse{0};		///< last system time that a range sample was fused by the terrain estimator
-	bool _hagl_valid{false};		///< true when the height above ground estimate is valid
 	terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground
 
 	// height sensor status
@@ -766,7 +763,6 @@ private:
 	void fuseFlowForTerrain();
 
 	void controlHaglFakeFusion();
-	void resetHaglFake();
 
 	// reset the heading and magnetic field states using the declination and magnetometer measurements
 	// return true if successful

src/modules/ekf2/EKF/terrain_estimator.cpp

@@ -45,7 +45,14 @@
 
 void Ekf::initHagl()
 {
-	resetHaglFake();
+	stopHaglFlowFusion();
+	stopHaglRngFusion();
+
+	// assume a ground clearance
+	_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
+
+	// use the ground clearance value as our uncertainty
+	_terrain_var = sq(_params.rng_gnd_clearance);
 }
 
 void Ekf::runTerrainEstimator()
@@ -66,8 +73,6 @@ void Ekf::runTerrainEstimator()
 	if (_terrain_vpos - _state.pos(2) < _params.rng_gnd_clearance) {
 		_terrain_vpos = _params.rng_gnd_clearance + _state.pos(2);
 	}
-
-	updateTerrainValidity();
 }
 
 void Ekf::predictHagl()
@@ -306,7 +311,7 @@ void Ekf::stopHaglFlowFusion()
 void Ekf::resetHaglFlow()
 {
 	// TODO: use the flow data
-	_terrain_vpos = fmaxf(0.0f,  _state.pos(2));
+	_terrain_vpos = fmaxf(0.0f, _state.pos(2));
 	_terrain_var = 100.0f;
 	_terrain_vpos_reset_counter++;
 }
@@ -410,27 +415,23 @@ void Ekf::controlHaglFakeFusion()
 	if (!_control_status.flags.in_air
 	    && !_hagl_sensor_status.flags.range_finder
 	    && !_hagl_sensor_status.flags.flow) {
-		resetHaglFake();
+
+		initHagl();
 	}
 }
 
-void Ekf::resetHaglFake()
-{
-	// assume a ground clearance
-	_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
-	// use the ground clearance value as our uncertainty
-	_terrain_var = sq(_params.rng_gnd_clearance);
-	_time_last_hagl_fuse = _imu_sample_delayed.time_us;
-}
-
-void Ekf::updateTerrainValidity()
+bool Ekf::isTerrainEstimateValid() const
 {
 	// we have been fusing range finder measurements in the last 5 seconds
-	const bool recent_range_fusion = isRecent(_time_last_hagl_fuse, (uint64_t)5e6);
+	if (_hagl_sensor_status.flags.range_finder && isRecent(_time_last_hagl_fuse, (uint64_t)5e6)) {
+		return true;
+	}
 
 	// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds
 	// this can only be the case if the main filter does not fuse optical flow
-	const bool recent_flow_for_terrain_fusion = isRecent(_time_last_flow_terrain_fuse, (uint64_t)5e6);
+	if (_hagl_sensor_status.flags.flow && isRecent(_time_last_flow_terrain_fuse, (uint64_t)5e6)) {
+		return true;
+	}
 
-	_hagl_valid = (recent_range_fusion || recent_flow_for_terrain_fusion);
+	return false;
 }