Compute the timestamp of average sample correctly

EKF/common.h

@@ -343,11 +343,11 @@ struct parameters {
 	int32_t valid_timeout_max{5000000};	///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
 
 	// static barometer pressure position error coefficient along body axes
-	float static_pressure_coef_xp {0.0f};
-	float static_pressure_coef_xn {0.0f};
-	float static_pressure_coef_yp {0.0f};
-	float static_pressure_coef_yn {0.0f};
-	float static_pressure_coef_z {0.0f};
+	float static_pressure_coef_xp {0.0f};	// (-)
+	float static_pressure_coef_xn {0.0f};	// (-)
+	float static_pressure_coef_yp {0.0f};	// (-)
+	float static_pressure_coef_yn {0.0f};	// (-)
+	float static_pressure_coef_z {0.0f};	// (-)
 	// upper limit on airspeed used for correction  (m/s**2)
 	float max_correction_airspeed {20.0f};
 

EKF/estimator_interface.cpp

@@ -264,6 +264,7 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float baro_alt_meter)
 	// by baro data by taking the average of incoming sample
 	_baro_sample_count++;
 	_baro_alt_sum += baro_alt_meter;
+	_baro_timestamp_sum += time_usec;
 
 	// limit data rate to prevent data being lost
 	if ((time_usec - _time_last_baro) > _min_obs_interval_us) {
@@ -274,7 +275,7 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float baro_alt_meter)
 		baro_sample_new.hgt = compensateBaroForDynamicPressure(baro_alt_avg);
 
 		// Use the time in the middle of the downsampling interval for the sample
-		baro_sample_new.time_us = _time_last_baro + (time_usec - _time_last_baro) / 2;
+		baro_sample_new.time_us = _baro_timestamp_sum / _baro_sample_count;
 		baro_sample_new.time_us -= _params.baro_delay_ms * 1000;
 		baro_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
 		baro_sample_new.time_us = math::max(baro_sample_new.time_us, _imu_sample_delayed.time_us);
@@ -284,6 +285,7 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float baro_alt_meter)
 		_time_last_baro = time_usec;
 		_baro_sample_count = 0;
 		_baro_alt_sum = 0.0f;
+		_baro_timestamp_sum = 0;
 	}
 }
 

EKF/estimator_interface.h

@@ -550,8 +550,9 @@ protected:
 	uint64_t _mag_timestamp_sum {0};
 
 	// Used to down sample barometer data
-	float _baro_alt_sum {0.0f};			///< summed pressure altitude readings (m)
-	uint8_t _baro_sample_count {0};		///< number of barometric altitude measurements summed
+	float _baro_alt_sum {0.0f};			// summed pressure altitude readings (m)
+	uint8_t _baro_sample_count {0};		// number of barometric altitude measurements summed
+	uint64_t _baro_timestamp_sum {0};	// summed timestamp to provide the timestamp of the averaged sample
 
 	fault_status_u _fault_status{};