ekf2: reinit baro height on sensor or calibration change

- handle reset on delayed time horizon

src/modules/ekf2/EKF/baro_height_control.cpp

@@ -59,17 +59,18 @@ void Ekf::controlBaroHeightFusion()
 		const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
 
 		if (measurement_valid) {
-			if (_baro_counter == 0) {
+			if ((_baro_counter == 0) || baro_sample.reset) {
 				_baro_lpf.reset(measurement);
+				_baro_counter = 1;
 
 			} else {
 				_baro_lpf.update(measurement);
+				_baro_counter++;
 			}
 
 			if (_baro_counter <= _obs_buffer_length) {
 				// Initialize the pressure offset (included in the baro bias)
 				bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
-				_baro_counter++;
 			}
 		}
 
@@ -108,10 +109,10 @@ void Ekf::controlBaroHeightFusion()
 		// determine if we should use height aiding
 		const bool continuing_conditions_passing = (_params.baro_ctrl == 1)
 				&& measurement_valid
+				&& (_baro_counter > _obs_buffer_length)
 				&& !_baro_hgt_faulty;
 
 		const bool starting_conditions_passing = continuing_conditions_passing
-				&& (_baro_counter > _obs_buffer_length)
 				&& isNewestSampleRecent(_time_last_baro_buffer_push, 2 * BARO_MAX_INTERVAL);
 
 		if (_control_status.flags.baro_hgt) {

src/modules/ekf2/EKF/common.h

@@ -222,6 +222,7 @@ struct magSample {
 struct baroSample {
 	uint64_t    time_us{};  ///< timestamp of the measurement (uSec)
 	float       hgt{};      ///< pressure altitude above sea level (m)
+	bool        reset{false};
 };
 
 struct rangeSample {

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -229,9 +229,8 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 	// limit data rate to prevent data being lost
 	if (time_us >= static_cast<int64_t>(_baro_buffer->get_newest().time_us + _min_obs_interval_us)) {
 
-		baroSample baro_sample_new;
+		baroSample baro_sample_new{baro_sample};
 		baro_sample_new.time_us = time_us;
-		baro_sample_new.hgt = baro_sample.hgt;
 
 		_baro_buffer->push(baro_sample_new);
 		_time_last_baro_buffer_push = _newest_high_rate_imu_sample.time_us;

src/modules/ekf2/EKF2.cpp

@@ -1712,28 +1712,25 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
 		// check if barometer has changed
 		if (airdata.baro_device_id != _device_id_baro) {
 			if (_device_id_baro != 0) {
-				PX4_WARN("%d - baro sensor ID changed %" PRIu32 " -> %" PRIu32, _instance, _device_id_baro, airdata.baro_device_id);
+				PX4_DEBUG("%d - baro sensor ID changed %" PRIu32 " -> %" PRIu32, _instance, _device_id_baro, airdata.baro_device_id);
 			}
 
 			reset = true;
 
-		} else if (airdata.calibration_count > _baro_calibration_count) {
+		} else if (airdata.calibration_count != _baro_calibration_count) {
 			// existing calibration has changed, reset saved baro bias
 			PX4_DEBUG("%d - baro %" PRIu32 " calibration updated, resetting bias", _instance, _device_id_baro);
 			reset = true;
 		}
 
 		if (reset) {
-			// TODO: reset baro ref and bias estimate?
 			_device_id_baro = airdata.baro_device_id;
 			_baro_calibration_count = airdata.calibration_count;
 		}
 
 		_ekf.set_air_density(airdata.rho);
 
-		_ekf.setBaroData(baroSample{airdata.timestamp_sample, airdata.baro_alt_meter});
-
-		_device_id_baro = airdata.baro_device_id;
+		_ekf.setBaroData(baroSample{airdata.timestamp_sample, airdata.baro_alt_meter, reset});
 
 		ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 -
 				(int64_t)ekf2_timestamps.timestamp / 100);