ekf2: remove baro accumulation from initializeFilter

src/modules/ekf2/EKF/baro_height_control.cpp

@@ -40,6 +40,28 @@
 
 void Ekf::controlBaroHeightFusion()
 {
+	if (!_baro_buffer) {
+		return;
+	}
+
+	baroSample baro_sample;
+	const bool baro_data_ready = _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &baro_sample);
+
+	if (baro_data_ready) {
+		if (_baro_counter == 0) {
+			_baro_lpf.reset(baro_sample.hgt);
+
+		} else {
+			_baro_lpf.update(baro_sample.hgt);
+		}
+
+		if (_baro_counter < _obs_buffer_length) {
+			// Initialize the pressure offset (included in the baro bias)
+			_baro_b_est.setBias(_state.pos(2) + _baro_lpf.getState());
+			_baro_counter++;
+		}
+	}
+
 	if (!(_params.baro_ctrl == 1)) {
 		stopBaroHgtFusion();
 		return;
@@ -50,12 +72,11 @@ void Ekf::controlBaroHeightFusion()
 	// check for intermittent data
 	const bool baro_hgt_intermittent = !isNewestSampleRecent(_time_last_baro_buffer_push, 2 * BARO_MAX_INTERVAL);
 
-	if (_baro_data_ready) {
-		_baro_lpf.update(_baro_sample_delayed.hgt);
-		updateBaroHgt(_baro_sample_delayed, _aid_src_baro_hgt);
+	if (baro_data_ready) {
+		updateBaroHgt(baro_sample, _aid_src_baro_hgt);
 
 		const bool continuing_conditions_passing = !_baro_hgt_faulty && !baro_hgt_intermittent;
-		const bool starting_conditions_passing = continuing_conditions_passing;
+		const bool starting_conditions_passing = continuing_conditions_passing && (_baro_counter >= _obs_buffer_length);
 
 		if (_control_status.flags.baro_hgt) {
 			if (continuing_conditions_passing) {
@@ -65,7 +86,7 @@ void Ekf::controlBaroHeightFusion()
 
 				if (isHeightResetRequired()) {
 					// All height sources are failing
-					resetHeightToBaro();
+					resetHeightToBaro(baro_sample);
 					resetVerticalVelocityToZero();
 
 				} else if (is_fusion_failing) {
@@ -79,7 +100,7 @@ void Ekf::controlBaroHeightFusion()
 			}
 		} else {
 			if (starting_conditions_passing) {
-				startBaroHgtFusion();
+				startBaroHgtFusion(baro_sample);
 			}
 		}
 
@@ -89,12 +110,12 @@ void Ekf::controlBaroHeightFusion()
 	}
 }
 
-void Ekf::startBaroHgtFusion()
+void Ekf::startBaroHgtFusion(const baroSample &baro_sample)
 {
 	if (!_control_status.flags.baro_hgt) {
 		if (_params.height_sensor_ref == HeightSensor::BARO) {
 			_height_sensor_ref = HeightSensor::BARO;
-			resetHeightToBaro();
+			resetHeightToBaro(baro_sample);
 
 		} else {
 			_baro_b_est.setBias(_state.pos(2) + _baro_lpf.getState());
@@ -106,12 +127,12 @@ void Ekf::startBaroHgtFusion()
 	}
 }
 
-void Ekf::resetHeightToBaro()
+void Ekf::resetHeightToBaro(const baroSample &baro_sample)
 {
 	ECL_INFO("reset height to baro");
 	_information_events.flags.reset_hgt_to_baro = true;
 
-	resetVerticalPositionTo(-(_baro_sample_delayed.hgt - _baro_b_est.getBias()));
+	resetVerticalPositionTo(-(baro_sample.hgt - _baro_b_est.getBias()));
 
 	// the state variance is the same as the observation
 	P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise));

src/modules/ekf2/EKF/control.cpp

@@ -85,18 +85,6 @@ void Ekf::controlFusionModes()
 		}
 	}
 
-	if (_baro_buffer) {
-		const uint64_t baro_time_prev = _baro_sample_delayed.time_us;
-		_baro_data_ready = _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed);
-
-		// if we have a new baro sample save the delta time between this sample and the last sample which is
-		// used below for baro offset calculations
-		if (_baro_data_ready && baro_time_prev != 0) {
-			_delta_time_baro_us = _baro_sample_delayed.time_us - baro_time_prev;
-		}
-	}
-
-
 	if (_gps_buffer) {
 		_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL);
 

src/modules/ekf2/EKF/ekf.cpp

@@ -160,33 +160,6 @@ bool Ekf::initialiseFilter()
 		}
 	}
 
-	// accumulate enough height measurements to be confident in the quality of the data
-	if (_baro_buffer) {
-		baroSample baro_sample;
-
-		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);
-
-				} else {
-					_baro_lpf.update(baro_sample.hgt);
-
-					_baro_b_est.setBias(_baro_lpf.getState());
-				}
-
-				_baro_counter++;
-			}
-		}
-	}
-
-	if (_params.baro_ctrl != 0) {
-		if (_baro_counter < _obs_buffer_length) {
-			// not enough baro samples accumulated
-			return false;
-		}
-	}
-
 	if (!initialiseTilt()) {
 		return false;
 	}

src/modules/ekf2/EKF/ekf.h

@@ -457,7 +457,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 _baro_data_ready{false};	///< true when new baro height 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 _ev_data_ready{false};	///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused
@@ -483,7 +482,6 @@ private:
 	Vector3f _last_known_pos{};		///< last known local position vector (m)
 
 	uint64_t _time_acc_bias_check{0};	///< last time the  accel bias check passed (uSec)
-	uint64_t _delta_time_baro_us{0};	///< delta time between two consecutive delayed baro samples from the buffer (uSec)
 
 	Vector3f _earth_rate_NED{};	///< earth rotation vector (NED) in rad/s
 
@@ -708,7 +706,7 @@ private:
 
 	void resetVerticalPositionTo(float new_vert_pos);
 
-	void resetHeightToBaro();
+	void resetHeightToBaro(const baroSample &baro_sample);
 	void resetHeightToGps(const gpsSample &gps_sample);
 	void resetHeightToRng();
 	void resetHeightToEv();
@@ -967,7 +965,7 @@ private:
 	void startMagHdgFusion();
 	void startMag3DFusion();
 
-	void startBaroHgtFusion();
+	void startBaroHgtFusion(const baroSample &baro_sample);
 	void stopBaroHgtFusion();
 
 	void startGpsHgtFusion(const gpsSample &gps_sample);

src/modules/ekf2/EKF/height_fusion.cpp

@@ -48,7 +48,7 @@ void Ekf::updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s
 
 	// measurement variance - user parameter defined
 	const float measurement_var = sq(fmaxf(_params.baro_noise, 0.01f));
-	const float measurement = _baro_sample_delayed.hgt;
+	const float measurement = baro_sample.hgt;
 
 	// vertical position innovation - baro measurement has opposite sign to earth z axis
 	baro_hgt.observation = -(measurement - _baro_b_est.getBias());