diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp
index fd1f1cfe92..ea57cb39f8 100644
--- a/src/modules/ekf2/EKF/control.cpp
+++ b/src/modules/ekf2/EKF/control.cpp
@@ -825,6 +825,17 @@ void Ekf::controlHeightFusion()
 		break;
 
 	case VDIST_SENSOR_RANGE:
+		// If we are supposed to be using range finder data as the primary height sensor, have bad range measurements
+		// and are on the ground, then synthesise a measurement at the expected on ground value
+		if (!_control_status.flags.in_air
+		    && !_range_sensor.isDataHealthy()
+		    && _range_sensor.isRegularlySendingData()
+		    && _range_sensor.isDataReady()) {
+
+			_range_sensor.setRange(_params.rng_gnd_clearance);
+			_range_sensor.setValidity(true); // bypass the checks
+		}
+
 		if (!_control_status.flags.rng_hgt) {
 			if (_range_sensor.isDataHealthy()) {
 				startRngHgtFusion();
@@ -887,18 +898,6 @@ void Ekf::controlHeightFusion()
 
 		if (_range_sensor.isDataHealthy()) {
 			fuseRngHgt();
-
-		} else if (!_control_status.flags.in_air
-			   && _range_sensor.isRegularlySendingData()
-			   && isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL)) {
-
-			// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
-			// and are on the ground, then synthesise a measurement at the expected on ground value
-			_range_sensor.setRange(_params.rng_gnd_clearance);
-			_range_sensor.setDataReadiness(true);
-			_range_sensor.setValidity(true); // bypass the checks
-
-			fuseRngHgt();
 		}
 
 	} else if (_control_status.flags.ev_hgt) {
diff --git a/src/modules/ekf2/EKF/sensor_range_finder.hpp b/src/modules/ekf2/EKF/sensor_range_finder.hpp
index ea123805df..c9dcaa03aa 100644
--- a/src/modules/ekf2/EKF/sensor_range_finder.hpp
+++ b/src/modules/ekf2/EKF/sensor_range_finder.hpp
@@ -58,6 +58,7 @@ public:
 	void runChecks(uint64_t current_time_us, const matrix::Dcmf &R_to_earth);
 	bool isHealthy() const override { return _is_sample_valid; }
 	bool isDataHealthy() const override { return _is_sample_ready && _is_sample_valid; }
+	bool isDataReady() const { return _is_sample_ready; }
 	bool isRegularlySendingData() const override { return _is_regularly_sending_data; }
 
 	void setSample(const rangeSample &sample)