ekf2: Prevent loss of baro data due to buffer time arrival checks

Baro data arriving too soon after the last measurement due to a high sampling rate or timing jitter is rejected inside the ecl EKF to prevent the data buffer overflowing.
This patch checks the timestamp difference from the last measurement, and if to small, the data is accumulated and the average sent to the EKF when the time delta
is acceptable.

src/modules/ekf2/ekf2_main.cpp

@@ -145,6 +145,12 @@ private:
 	uint8_t _mag_sample_count = 0;		// number of magnetometer measurements summed
 	uint32_t _mag_time_ms_last_used = 0;	// time stamp in msec of the last averaged magnetometer measurement used by the EKF
 
+	// Used to down sample barometer data
+	float _balt_data_sum;			// summed barometric altitude readings (m)
+	uint64_t _balt_time_sum_ms;		// summed barometric altitude time stamps (msec)
+	uint8_t _balt_sample_count = 0;		// number of barometric altitude measurements summed
+	uint32_t _balt_time_ms_last_used = 0;	// time stamp in msec of the last averaged barometric altitude measurement used by the EKF
+
 	int	_sensors_sub = -1;
 	int	_gps_sub = -1;
 	int	_airspeed_sub = -1;
@@ -572,8 +578,22 @@ void Ekf2::task_main()
 		} else {
 			if ((sensors.timestamp + sensors.baro_timestamp_relative) != _timestamp_balt_us) {
 				_timestamp_balt_us = sensors.timestamp + sensors.baro_timestamp_relative;
-			_ekf.setBaroData(_timestamp_balt_us, &sensors.baro_alt_meter);
 
+				// If the time last used by the EKF is less than 50msec, then accumulate the
+				// data and push the average when the 50msec is reached.
+				_balt_time_sum_ms += _timestamp_balt_us/1000;
+				_balt_sample_count++;
+				_balt_data_sum += sensors.baro_alt_meter;
+				uint64_t balt_time_ms = _balt_time_sum_ms / _balt_sample_count;
+				if (balt_time_ms - _balt_time_ms_last_used > 50) {
+					float balt_data_avg = _balt_data_sum / (float)_balt_sample_count;
+					_ekf.setBaroData(1000*(uint64_t)balt_time_ms, &balt_data_avg);
+					_balt_time_ms_last_used = balt_time_ms;
+					_balt_time_sum_ms = 0;
+					_balt_sample_count = 0;
+					_balt_data_sum = 0.0f;
+
+				}
 			}
 		}