hit me

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 {

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();
 				}

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

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) {