ekf2: continuing updating baro offset if never moved

src/modules/ekf2/EKF/control.cpp

@@ -85,6 +85,11 @@ void Ekf::controlFusionModes()
 		}
 	}
 
+	if (_never_moved && (!_control_status.flags.vehicle_at_rest || _control_status.flags.in_air)) {
+		PX4_WARN("moved");
+		_never_moved = false;
+	}
+
 	if (_baro_buffer) {
 		// check for intermittent data
 		_baro_hgt_intermittent = !isRecent(_time_last_baro, 2 * BARO_MAX_INTERVAL);
@@ -92,6 +97,16 @@ void Ekf::controlFusionModes()
 		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 (_baro_data_ready) {
+			_baro_lpf.update(_baro_sample_delayed.hgt);
+			_baro_counter++;
+		}
+
+		if (_never_moved && _control_status.flags.baro_hgt) {
+			// continue updating baro height offset if we've never moved
+			_baro_hgt_offset = _baro_lpf.getState();
+		}
+
 		// 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) {

src/modules/ekf2/EKF/ekf.cpp

@@ -85,6 +85,8 @@ void Ekf::reset()
 
 	_prev_dvel_bias_var.zero();
 
+	_never_moved = true;
+
 	resetGpsDriftCheckFilters();
 }
 
@@ -164,10 +166,11 @@ bool Ekf::initialiseFilter()
 	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);
 				_baro_hgt_offset = _baro_sample_delayed.hgt;
 
 			} else {
-				_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
+				_baro_hgt_offset = _baro_lpf.update(_baro_sample_delayed.hgt);
 			}
 
 			_baro_counter++;
@@ -175,13 +178,13 @@ bool Ekf::initialiseFilter()
 	}
 
 	if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
-		if (_mag_counter < _obs_buffer_length) {
+		if (_mag_counter == 0) {
 			// not enough mag samples accumulated
 			return false;
 		}
 	}
 
-	if (_baro_counter < _obs_buffer_length) {
+	if (_baro_counter == 0) {
 		// not enough baro samples accumulated
 		return false;
 	}

src/modules/ekf2/EKF/ekf.h

@@ -506,6 +506,9 @@ private:
 	uint32_t _mag_counter{0};		///< number of magnetometer samples read during initialisation
 	AlphaFilter<Vector3f> _accel_lpf{0.1f};	///< filtered accelerometer measurement used to align tilt (m/s/s)
 	AlphaFilter<Vector3f> _gyro_lpf{0.1f};	///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
+	AlphaFilter<float> _baro_lpf{0.1f};
+
+	bool _never_moved{true};
 
 	// Variables used to perform in flight resets and switch between height sources
 	AlphaFilter<Vector3f> _mag_lpf{0.1f};	///< filtered magnetometer measurement for instant reset (Gauss)