ekf: calculate the delta time between consecutive baro measurments

(used for calculating filtered baro offset when primary height source
is not baro)

Signed-off-by: Roman <bapstroman@gmail.com>

EKF/control.cpp

@@ -82,8 +82,16 @@ void Ekf::controlFusionModes()
 	// check for arrival of new sensor data at the fusion time horizon
 	_gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
 	_mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
+
+	_delta_time_baro_us = _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) {
+		_delta_time_baro_us = _baro_sample_delayed.time_us - _delta_time_baro_us;
+	}
+
 	// calculate 2,2 element of rotation matrix from sensor frame to earth frame
 	_R_rng_to_earth_2_2 = _R_to_earth(2, 0) * _sin_tilt_rng + _R_to_earth(2, 2) * _cos_tilt_rng;
 	_range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)

EKF/ekf.h

@@ -261,6 +261,7 @@ private:
 	float _imu_collection_time_adj{0.0f};	// the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
 
 	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