diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp
index c25a3d0a34..c355429d9f 100644
--- a/src/modules/ekf2/EKF/control.cpp
+++ b/src/modules/ekf2/EKF/control.cpp
@@ -130,50 +130,6 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 		}
 	}
 
-	if (_range_buffer) {
-		// Get range data from buffer and check validity
-		_rng_data_ready = _range_buffer->pop_first_older_than(imu_delayed.time_us, _range_sensor.getSampleAddress());
-		_range_sensor.setDataReadiness(_rng_data_ready);
-
-		// update range sensor angle parameters in case they have changed
-		_range_sensor.setPitchOffset(_params.rng_sens_pitch);
-		_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
-		_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
-
-		_range_sensor.runChecks(imu_delayed.time_us, _R_to_earth);
-
-		if (_range_sensor.isDataHealthy()) {
-			// correct the range data for position offset relative to the IMU
-			const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
-			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-			_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
-
-			// Run the kinematic consistency check when not moving horizontally
-			if (_control_status.flags.in_air && !_control_status.flags.fixed_wing
-			    && (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) {
-
-				const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
-				const float var = sq(_params.range_noise) + dist_dependant_var;
-
-				_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
-				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), imu_delayed.time_us);
-			}
-
-		} else {
-			// 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.isRegularlySendingData()
-			&& _range_sensor.isDataReady()) {
-
-				_range_sensor.setRange(_params.rng_gnd_clearance);
-				_range_sensor.setValidity(true); // bypass the checks
-			}
-		}
-
-		_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
-	}
-
 	// run EKF-GSF yaw estimator once per imu_delayed update after all main EKF data samples available
 	runYawEKFGSF(imu_delayed);
 
diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index 7f80a076c9..f215fb7db7 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/src/modules/ekf2/EKF/ekf.h
@@ -488,7 +488,6 @@ private:
 
 	// booleans true when fresh sensor data is available at the fusion time horizon
 	bool _gps_data_ready{false};	///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
-	bool _rng_data_ready{false};
 	bool _flow_data_ready{false};	///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
 	bool _tas_data_ready{false};	///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused
 	bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator
diff --git a/src/modules/ekf2/EKF/range_height_control.cpp b/src/modules/ekf2/EKF/range_height_control.cpp
index 5965462c0d..88a9cb7197 100644
--- a/src/modules/ekf2/EKF/range_height_control.cpp
+++ b/src/modules/ekf2/EKF/range_height_control.cpp
@@ -42,12 +42,61 @@ void Ekf::controlRangeHeightFusion()
 {
 	static constexpr const char *HGT_SRC_NAME = "RNG";
 
+	bool rng_data_ready = false;
+
+	if (_range_buffer) {
+		// Get range data from buffer and check validity
+		rng_data_ready = _range_buffer->pop_first_older_than(_time_delayed_us, _range_sensor.getSampleAddress());
+		_range_sensor.setDataReadiness(rng_data_ready);
+
+		// update range sensor angle parameters in case they have changed
+		_range_sensor.setPitchOffset(_params.rng_sens_pitch);
+		_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
+		_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
+
+		_range_sensor.runChecks(_time_delayed_us, _R_to_earth);
+
+		if (_range_sensor.isDataHealthy()) {
+			// correct the range data for position offset relative to the IMU
+			const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
+			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+			_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
+
+			// Run the kinematic consistency check when not moving horizontally
+			if (_control_status.flags.in_air && !_control_status.flags.fixed_wing
+			    && (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) {
+
+				const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
+				const float var = sq(_params.range_noise) + dist_dependant_var;
+
+				_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
+				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), _time_delayed_us);
+			}
+
+		} else {
+			// 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.isRegularlySendingData()
+			    && _range_sensor.isDataReady()) {
+
+				_range_sensor.setRange(_params.rng_gnd_clearance);
+				_range_sensor.setValidity(true); // bypass the checks
+			}
+		}
+
+		_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
+
+	} else {
+		return;
+	}
+
 	auto &aid_src = _aid_src_rng_hgt;
 	HeightBiasEstimator &bias_est = _rng_hgt_b_est;
 
 	bias_est.predict(_dt_ekf_avg);
 
-	if (_rng_data_ready && _range_sensor.getSampleAddress()) {
+	if (rng_data_ready && _range_sensor.getSampleAddress()) {
 
 		const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
 		const float measurement_var = sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom());