From dc2bfca2b67c1df20ca86a2c3e3cd7671ec1c5d9 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 14 Mar 2023 17:52:31 -0400
Subject: [PATCH] hit me

---
 src/modules/ekf2/EKF/common.h                 |  1 -
 src/modules/ekf2/EKF/optical_flow_control.cpp |  2 +-
 src/modules/ekf2/EKF/terrain_estimator.cpp    | 12 +++++++++---
 src/modules/ekf2/EKF2.cpp                     |  3 +--
 4 files changed, 11 insertions(+), 7 deletions(-)

diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h
index 9b4a854c67..c75ed2dae7 100644
--- a/src/modules/ekf2/EKF/common.h
+++ b/src/modules/ekf2/EKF/common.h
@@ -236,7 +236,6 @@ struct flowSample {
 	Vector3f    gyro_xyz{};    ///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
 	float       dt{};          ///< amount of integration time (sec)
 	uint8_t     quality{};     ///< quality indicator between 0 and 255
-	float       ground_distance_m{NAN}; ///< optical range finder measurement (m) if available
 };
 
 struct extVisionSample {
diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp
index c403973354..117fc724ea 100644
--- a/src/modules/ekf2/EKF/optical_flow_control.cpp
+++ b/src/modules/ekf2/EKF/optical_flow_control.cpp
@@ -170,6 +170,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 		    && !_control_status.flags.opt_flow // we are not yet using flow data
 		    && !inhibit_flow_use
 		    && !isRecent(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6)
+		    && isTerrainEstimateValid()
 		   ) {
 
 			// set the flag and reset the fusion timeout
@@ -215,7 +216,6 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 				// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
 				// but use a relaxed time criteria to enable it to coast through bad range finder data
 				if (isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) {
-					updateOptFlow(_aid_src_optical_flow);
 					fuseOptFlow();
 					_last_known_pos.xy() = _state.pos.xy();
 				}
diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp
index 56900214fe..c084772448 100644
--- a/src/modules/ekf2/EKF/terrain_estimator.cpp
+++ b/src/modules/ekf2/EKF/terrain_estimator.cpp
@@ -53,6 +53,8 @@ void Ekf::initHagl()
 
 	// use the ground clearance value as our uncertainty
 	_terrain_var = sq(_params.rng_gnd_clearance);
+
+	_time_last_hagl_fuse = _time_delayed_us;
 }
 
 void Ekf::runTerrainEstimator(const imuSample &imu_delayed)
@@ -399,15 +401,19 @@ void Ekf::controlHaglFakeFusion()
 	    && !_hagl_sensor_status.flags.range_finder
 	    && !_hagl_sensor_status.flags.flow) {
 
-		initHagl();
+		if (_control_status.flags.vehicle_at_rest || isTimedOut(_time_last_hagl_fuse, (uint64_t)1e6)) {
+			initHagl();
+		}
 	}
 }
 
 bool Ekf::isTerrainEstimateValid() const
 {
 	// we have been fusing range finder measurements in the last 5 seconds
-	if (_hagl_sensor_status.flags.range_finder && isRecent(_time_last_hagl_fuse, (uint64_t)5e6)) {
-		return true;
+	if (isRecent(_time_last_hagl_fuse, (uint64_t)5e6)) {
+		if (_hagl_sensor_status.flags.range_finder || !_control_status.flags.in_air) {
+			return true;
+		}
 	}
 
 	// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds
diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index 98d34b1816..601ac28ce4 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -1971,8 +1971,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
 			.flow_xy_rad = Vector2f{-optical_flow.pixel_flow[0], -optical_flow.pixel_flow[1]},
 			.gyro_xyz = Vector3f{-optical_flow.delta_angle[0], -optical_flow.delta_angle[1], -optical_flow.delta_angle[2]},
 			.dt = 1e-6f * (float)optical_flow.integration_timespan_us,
-			.quality = optical_flow.quality,
-			.ground_distance_m = optical_flow.distance_m,
+			.quality = optical_flow.quality
 		};
 
 		if (Vector2f(optical_flow.pixel_flow).isAllFinite() && flow.dt < 1) {