diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp
index 9dae933cee..ca00fb794e 100644
--- a/src/modules/ekf2/EKF/ekf.cpp
+++ b/src/modules/ekf2/EKF/ekf.cpp
@@ -161,29 +161,32 @@ bool Ekf::initialiseFilter()
 	}
 
 	// accumulate enough height measurements to be confident in the quality of the data
-	if (_baro_buffer && _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
-		if (_baro_sample_delayed.time_us != 0) {
-			if (_baro_counter == 0) {
-				_baro_lpf.reset(_baro_sample_delayed.hgt);
+	if (_baro_buffer) {
+		baroSample baro_sample;
 
-			} else {
-				_baro_lpf.update(_baro_sample_delayed.hgt);
+		if (_baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &baro_sample)) {
+			if (baro_sample.time_us != 0) {
+				if (_baro_counter == 0) {
+					_baro_lpf.reset(baro_sample.hgt);
 
-				_baro_b_est.setBias(_baro_lpf.getState());
+				} else {
+					_baro_lpf.update(baro_sample.hgt);
+
+					_baro_b_est.setBias(_baro_lpf.getState());
+				}
+
+				_baro_counter++;
 			}
-
-			_baro_counter++;
 		}
 	}
 
-	if (_baro_counter < _obs_buffer_length) {
-		// not enough baro samples accumulated
-		return false;
+	if (_params.baro_ctrl != 0) {
+		if (_baro_counter < _obs_buffer_length) {
+			// not enough baro samples accumulated
+			return false;
+		}
 	}
 
-	// we use baro height initially and switch to GPS/range/EV finder later when it passes checks.
-	_control_status.flags.baro_hgt = false;
-
 	if (!initialiseTilt()) {
 		return false;
 	}