ekf2: improve ring buffer sizing and dynamically allocate

src/modules/ekf2/CMakeLists.txt

@@ -39,6 +39,7 @@ px4_add_module(
 	COMPILE_FLAGS
 		${MAX_CUSTOM_OPT_LEVEL}
 		-fno-associative-math
+		#-DDEBUG_BUILD
 	INCLUDES
 		EKF
 	STACK_MAX

src/modules/ekf2/EKF/RingBuffer.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2015-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -46,7 +46,7 @@ class RingBuffer
 {
 public:
 	explicit RingBuffer(size_t size) { allocate(size); }
-	RingBuffer() { allocate(1); }
+	RingBuffer() = delete;
 	~RingBuffer() { delete[] _buffer; }
 
 	// no copy, assignment, move, move assignment
@@ -155,6 +155,19 @@ public:
 
 	int get_total_size() const { return sizeof(*this) + sizeof(data_type) * _size; }
 
+	int entries() const
+	{
+		int count = 0;
+
+		for (uint8_t i = 0; i < _size; i++) {
+			if (_buffer[i].time_us != 0) {
+				count++;
+			}
+		}
+
+		return count;
+	}
+
 private:
 	data_type *_buffer{nullptr};
 

src/modules/ekf2/EKF/common.h

@@ -219,7 +219,8 @@ struct parameters {
 	int32_t vdist_sensor_type{VDIST_SENSOR_BARO};	///< selects the primary source for height data
 	int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
 				    TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
-	int32_t sensor_interval_min_ms{20};		///< minimum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec)
+
+	int32_t sensor_interval_max_ms{10};		///< maximum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec)
 
 	// measurement time delays
 	float mag_delay_ms{0.0f};		///< magnetometer measurement delay relative to the IMU (mSec)

src/modules/ekf2/EKF/control.cpp

@@ -85,47 +85,59 @@ void Ekf::controlFusionModes()
 		}
 	}
 
-	// check for intermittent data (before pop_first_older_than)
-	const baroSample &baro_init = _baro_buffer.get_newest();
-	_baro_hgt_faulty = !isRecent(baro_init.time_us, 2 * BARO_MAX_INTERVAL);
+	if (_baro_buffer) {
+		// check for intermittent data (before pop_first_older_than)
+		const baroSample &baro_init = _baro_buffer->get_newest();
+		_baro_hgt_faulty = !isRecent(baro_init.time_us, 2 * BARO_MAX_INTERVAL);
 
-	const gpsSample &gps_init = _gps_buffer.get_newest();
-	_gps_hgt_intermittent = !isRecent(gps_init.time_us, 2 * GPS_MAX_INTERVAL);
+		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);
 
-	// check for arrival of new sensor data at the fusion time horizon
-	_time_prev_gps_us = _gps_sample_delayed.time_us;
-	_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);
-
-	if (_mag_data_ready) {
-		_mag_lpf.update(_mag_sample_delayed.mag);
-
-		// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
-		// this is useful if there is a lot of interference on the sensor measurement.
-		if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL) && (_NED_origin_initialised
-				|| PX4_ISFINITE(_mag_declination_gps))) {
-
-			const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
-			_mag_sample_delayed.mag(2) = calculate_synthetic_mag_z_measurement(_mag_sample_delayed.mag, mag_earth_pred);
-			_control_status.flags.synthetic_mag_z = true;
-
-		} else {
-			_control_status.flags.synthetic_mag_z = false;
+		// 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) {
+			_delta_time_baro_us = _baro_sample_delayed.time_us - baro_time_prev;
 		}
 	}
 
-	_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;
+	if (_gps_buffer) {
+		const gpsSample &gps_init = _gps_buffer->get_newest();
+		_gps_hgt_intermittent = !isRecent(gps_init.time_us, 2 * GPS_MAX_INTERVAL);
+
+		// check for arrival of new sensor data at the fusion time horizon
+		_time_prev_gps_us = _gps_sample_delayed.time_us;
+		_gps_data_ready = _gps_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
 	}
 
-	{
+
+	if (_mag_buffer) {
+		_mag_data_ready = _mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
+
+		if (_mag_data_ready) {
+			_mag_lpf.update(_mag_sample_delayed.mag);
+
+			// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
+			// this is useful if there is a lot of interference on the sensor measurement.
+			if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL)
+			    && (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps))
+			   ) {
+
+				const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
+				_mag_sample_delayed.mag(2) = calculate_synthetic_mag_z_measurement(_mag_sample_delayed.mag, mag_earth_pred);
+				_control_status.flags.synthetic_mag_z = true;
+
+			} else {
+				_control_status.flags.synthetic_mag_z = false;
+			}
+		}
+
+
+	}
+
+	if (_range_buffer) {
 		// Get range data from buffer and check validity
-		const bool is_rng_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
+		bool is_rng_data_ready = _range_buffer->pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
 		_range_sensor.setDataReadiness(is_rng_data_ready);
 
 		// update range sensor angle parameters in case they have changed
@@ -134,35 +146,42 @@ void Ekf::controlFusionModes()
 		_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
 
 		_range_sensor.runChecks(_imu_sample_delayed.time_us, _R_to_earth);
+
+		if (_range_sensor.isDataHealthy()) {
+			// correct the range data for position offset relative to the IMU
+			const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
+			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+			_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
+		}
 	}
 
-	if (_range_sensor.isDataHealthy()) {
-		// correct the range data for position offset relative to the IMU
-		const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
-		const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-		_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
+	if (_flow_buffer) {
+		// We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon.
+		// This means we stop looking for new data until the old data has been fused, unless we are not fusing optical flow,
+		// in this case we need to empty the buffer
+		if (!_flow_data_ready || !_control_status.flags.opt_flow) {
+			_flow_data_ready = _flow_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed);
+		}
+
+		// check if we should fuse flow data for terrain estimation
+		if (!_flow_for_terrain_data_ready && _flow_data_ready && _control_status.flags.in_air) {
+			// TODO: WARNING, _flow_data_ready can be modified in controlOpticalFlowFusion
+			// due to some checks failing
+			// only fuse flow for terrain if range data hasn't been fused for 5 seconds
+			_flow_for_terrain_data_ready = isTimedOut(_time_last_hagl_fuse, (uint64_t)5E6);
+			// only fuse flow for terrain if the main filter is not fusing flow and we are using gps
+			_flow_for_terrain_data_ready &= (!_control_status.flags.opt_flow && _control_status.flags.gps);
+		}
 	}
 
-	// We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon.
-	// This means we stop looking for new data until the old data has been fused, unless we are not fusing optical flow,
-	// in this case we need to empty the buffer
-	if (!_flow_data_ready || !_control_status.flags.opt_flow) {
-		_flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed);
+	if (_ext_vision_buffer) {
+		_ev_data_ready = _ext_vision_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
 	}
 
-	// check if we should fuse flow data for terrain estimation
-	if (!_flow_for_terrain_data_ready && _flow_data_ready && _control_status.flags.in_air) {
-		// TODO: WARNING, _flow_data_ready can be modified in controlOpticalFlowFusion
-		// due to some checks failing
-		// only fuse flow for terrain if range data hasn't been fused for 5 seconds
-		_flow_for_terrain_data_ready = isTimedOut(_time_last_hagl_fuse, (uint64_t)5E6);
-		// only fuse flow for terrain if the main filter is not fusing flow and we are using gps
-		_flow_for_terrain_data_ready &= (!_control_status.flags.opt_flow && _control_status.flags.gps);
+	if (_airspeed_buffer) {
+		_tas_data_ready = _airspeed_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed);
 	}
 
-	_ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
-	_tas_data_ready = _airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed);
-
 	// check for height sensor timeouts and reset and change sensor if necessary
 	controlHeightSensorTimeouts();
 
@@ -632,8 +651,12 @@ void Ekf::controlHeightSensorTimeouts()
 
 		if (_control_status.flags.baro_hgt) {
 			// check if GPS height is available
-			const gpsSample &gps_init = _gps_buffer.get_newest();
-			const bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
+			bool gps_hgt_accurate = false;
+
+			if (_gps_buffer) {
+				const gpsSample &gps_init = _gps_buffer->get_newest();
+				gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
+			}
 
 			// check for inertial sensing errors in the last BADACC_PROBATION seconds
 			const bool prev_bad_vert_accel = isRecent(_time_bad_vert_accel, BADACC_PROBATION);
@@ -660,13 +683,21 @@ void Ekf::controlHeightSensorTimeouts()
 
 		} else if (_control_status.flags.gps_hgt) {
 			// check if GPS height is available
-			const gpsSample &gps_init = _gps_buffer.get_newest();
-			const bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
+			bool gps_hgt_accurate = false;
+
+			if (_gps_buffer) {
+				const gpsSample &gps_init = _gps_buffer->get_newest();
+				gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
+			}
 
 			// check the baro height source for consistency and freshness
-			const baroSample &baro_init = _baro_buffer.get_newest();
-			const float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
-			const bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9, 9)) * sq(_params.baro_innov_gate);
+			bool baro_data_consistent = false;
+
+			if (_baro_buffer) {
+				const baroSample &baro_init = _baro_buffer->get_newest();
+				const float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
+				baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9, 9)) * sq(_params.baro_innov_gate);
+			}
 
 			// if baro data is acceptable and GPS data is inaccurate, reset height to baro
 			const bool reset_to_baro = !_baro_hgt_faulty &&
@@ -702,8 +733,12 @@ void Ekf::controlHeightSensorTimeouts()
 
 		} else if (_control_status.flags.ev_hgt) {
 			// check if vision data is available
-			const extVisionSample &ev_init = _ext_vision_buffer.get_newest();
-			const bool ev_data_available = isRecent(ev_init.time_us, 2 * EV_MAX_INTERVAL);
+			bool ev_data_available = false;
+
+			if (_ext_vision_buffer) {
+				const extVisionSample &ev_init = _ext_vision_buffer->get_newest();
+				ev_data_available = isRecent(ev_init.time_us, 2 * EV_MAX_INTERVAL);
+			}
 
 			if (ev_data_available) {
 				request_height_reset = true;
@@ -784,7 +819,7 @@ void Ekf::checkVerticalAccelerationHealth()
 	const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || is_clipping_frequently) && is_inertial_nav_falling;
 
 	if (bad_vert_accel) {
-		_time_bad_vert_accel =  _time_last_imu;
+		_time_bad_vert_accel = _time_last_imu;
 
 	} else {
 		_time_good_vert_accel = _time_last_imu;
@@ -807,7 +842,7 @@ void Ekf::controlHeightFusion()
 
 	switch (_params.vdist_sensor_type) {
 	default:
-		ECL_ERR("Invalid hgt mode: %d", _params.vdist_sensor_type);
+		ECL_ERR("Invalid hgt mode: %" PRIi32, _params.vdist_sensor_type);
 
 	// FALLTHROUGH
 	case VDIST_SENSOR_BARO:
@@ -825,6 +860,7 @@ void Ekf::controlHeightFusion()
 		break;
 
 	case VDIST_SENSOR_RANGE:
+
 		// If we are supposed to be using range finder data as the primary height sensor, have bad range measurements
 		// and are on the ground, then synthesise a measurement at the expected on ground value
 		if (!_control_status.flags.in_air
@@ -845,6 +881,7 @@ void Ekf::controlHeightFusion()
 		break;
 
 	case VDIST_SENSOR_GPS:
+
 		// NOTE: emergency fallback due to extended loss of currently selected sensor data or failure
 		// to pass innovation cinsistency checks is handled elsewhere in Ekf::controlHeightSensorTimeouts.
 		// Do switching between GPS and rangefinder if using range finder as a height source when close
@@ -870,6 +907,7 @@ void Ekf::controlHeightFusion()
 		break;
 
 	case VDIST_SENSOR_EV:
+
 		// don't start using EV data unless data is arriving frequently
 		if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
 			startEvHgtFusion();
@@ -1022,10 +1060,9 @@ void Ekf::controlDragFusion()
 			_control_status.flags.wind = true;
 			resetWind();
 
-		} else if (_drag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) {
+		} else if (_drag_buffer && _drag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) {
 			fuseDrag();
 		}
-
 	}
 }
 
@@ -1089,7 +1126,11 @@ void Ekf::controlFakePosFusion()
 
 void Ekf::controlAuxVelFusion()
 {
-	const bool data_ready = _auxvel_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_auxvel_sample_delayed);
+	bool data_ready = false;
+
+	if (_auxvel_buffer) {
+		data_ready = _auxvel_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_auxvel_sample_delayed);
+	}
 
 	if (data_ready && isHorizontalAidingActive()) {
 

src/modules/ekf2/EKF/ekf.cpp

@@ -143,7 +143,7 @@ bool Ekf::initialiseFilter()
 	}
 
 	// Sum the magnetometer measurements
-	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
+	if (_mag_buffer && _mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
 		if (_mag_sample_delayed.time_us != 0) {
 			if (_mag_counter == 0) {
 				_mag_lpf.reset(_mag_sample_delayed.mag);
@@ -157,7 +157,7 @@ bool Ekf::initialiseFilter()
 	}
 
 	// accumulate enough height measurements to be confident in the quality of the data
-	if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
+	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_hgt_offset = _baro_sample_delayed.hgt;
@@ -325,10 +325,13 @@ void Ekf::calculateOutputStates(const imuSample &imu)
 	// Note fixed coefficients are used to save operations. The exact time constant is not important.
 	_yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu.delta_ang_dt;
 
+
+	_output_new.time_us = imu.time_us;
+	_output_vert_new.time_us = imu.time_us;
+
 	const Quatf dq(AxisAnglef{delta_angle});
 
 	// rotate the previous INS quaternion by the delta quaternions
-	_output_new.time_us = imu.time_us;
 	_output_new.quat_nominal = _output_new.quat_nominal * dq;
 
 	// the quaternions must always be normalised after modification

src/modules/ekf2/EKF/ekf.h

@@ -546,8 +546,8 @@ private:
 	terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground
 
 	// height sensor status
-	bool _baro_hgt_faulty{false};		///< true if valid baro data is unavailable for use
-	bool _gps_hgt_intermittent{false};	///< true if gps height into the buffer is intermittent
+	bool _baro_hgt_faulty{true};		///< true if valid baro data is unavailable for use
+	bool _gps_hgt_intermittent{true};	///< true if gps height into the buffer is intermittent
 
 	// imu fault status
 	uint64_t _time_bad_vert_accel{0};	///< last time a bad vertical accel was detected (uSec)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -255,9 +255,6 @@ void Ekf::resetVerticalPositionTo(const float &new_vert_pos)
 // Reset height state using the last height measurement
 void Ekf::resetHeight()
 {
-	// Get the most recent GPS data
-	const gpsSample &gps_newest = _gps_buffer.get_newest();
-
 	// reset the vertical position
 	if (_control_status.flags.rng_hgt) {
 		float dist_bottom;
@@ -277,15 +274,18 @@ void Ekf::resetHeight()
 		P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise));
 
 		// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
-		const baroSample &baro_newest = _baro_buffer.get_newest();
-		_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
+		if (_baro_buffer) {
+			const baroSample &baro_newest = _baro_buffer->get_newest();
+			_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
+		}
 
 	} else if (_control_status.flags.baro_hgt) {
 		// initialize vertical position with newest baro measurement
-		const baroSample &baro_newest = _baro_buffer.get_newest();
-
 		if (!_baro_hgt_faulty) {
-			resetVerticalPositionTo(-baro_newest.hgt + _baro_hgt_offset);
+			if (_baro_buffer) {
+				const baroSample &baro_newest = _baro_buffer->get_newest();
+				resetVerticalPositionTo(-baro_newest.hgt + _baro_hgt_offset);
+			}
 
 			// the state variance is the same as the observation
 			P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise));
@@ -296,15 +296,18 @@ void Ekf::resetHeight()
 
 	} else if (_control_status.flags.gps_hgt) {
 		// initialize vertical position and velocity with newest gps measurement
-		if (!_gps_hgt_intermittent) {
+		if (!_gps_hgt_intermittent && _gps_buffer) {
+			const gpsSample &gps_newest = _gps_buffer->get_newest();
 			resetVerticalPositionTo(_hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref);
 
 			// the state variance is the same as the observation
 			P.uncorrelateCovarianceSetVariance<1>(9, sq(gps_newest.vacc));
 
 			// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
-			const baroSample &baro_newest = _baro_buffer.get_newest();
-			_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
+			if (_baro_buffer) {
+				const baroSample &baro_newest = _baro_buffer->get_newest();
+				_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
+			}
 
 		} else {
 			// TODO: reset to last known gps based estimate
@@ -312,20 +315,23 @@ void Ekf::resetHeight()
 
 	} else if (_control_status.flags.ev_hgt) {
 		// initialize vertical position with newest measurement
-		const extVisionSample &ev_newest = _ext_vision_buffer.get_newest();
+		if (_ext_vision_buffer) {
+			const extVisionSample &ev_newest = _ext_vision_buffer->get_newest();
 
-		// use the most recent data if it's time offset from the fusion time horizon is smaller
-		if (ev_newest.time_us >= _ev_sample_delayed.time_us) {
-			resetVerticalPositionTo(ev_newest.pos(2));
+			// use the most recent data if it's time offset from the fusion time horizon is smaller
+			if (ev_newest.time_us >= _ev_sample_delayed.time_us) {
+				resetVerticalPositionTo(ev_newest.pos(2));
 
-		} else {
-			resetVerticalPositionTo(_ev_sample_delayed.pos(2));
+			} else {
+				resetVerticalPositionTo(_ev_sample_delayed.pos(2));
+			}
 		}
 	}
 
 	// reset the vertical velocity state
-	if (_control_status.flags.gps && !_gps_hgt_intermittent) {
+	if (_control_status.flags.gps && !_gps_hgt_intermittent && _gps_buffer) {
 		// If we are using GPS, then use it to reset the vertical velocity
+		const gpsSample &gps_newest = _gps_buffer->get_newest();
 		resetVerticalVelocityTo(gps_newest.vel(2));
 
 		// the state variance is the same as the observation
@@ -400,7 +406,7 @@ bool Ekf::realignYawGPS()
 	const bool badMagYaw = (badYawErr && badVelInnov);
 
 	if (badMagYaw) {
-		_num_bad_flight_yaw_events ++;
+		_num_bad_flight_yaw_events++;
 	}
 
 	// correct yaw angle using GPS ground course if compass yaw bad or yaw is previously not aligned
@@ -719,6 +725,7 @@ void Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 
 	// reinitialize map projection to latitude, longitude, altitude, and reset position
 	_pos_ref.initReference(latitude, longitude, _time_last_imu);
+
 	if (current_pos_available) {
 		// reset horizontal position
 		Vector2f position = _pos_ref.project(current_lat, current_lon);
@@ -1337,7 +1344,7 @@ void Ekf::updateBaroHgtOffset()
 {
 	// calculate a filtered offset between the baro origin and local NED origin if we are not
 	// using the baro as a height reference
-	if (!_control_status.flags.baro_hgt && _baro_data_ready) {
+	if (!_control_status.flags.baro_hgt && _baro_data_ready && (_delta_time_baro_us != 0)) {
 		const float local_time_step = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f);
 
 		// apply a 10 second first order low pass filter to baro offset
@@ -1361,7 +1368,7 @@ float Ekf::getGpsHeightVariance()
 void Ekf::updateBaroHgtBias()
 {
 	// Baro bias estimation using GPS altitude
-	if (_baro_data_ready) {
+	if (_baro_data_ready && (_delta_time_baro_us != 0)) {
 		const float dt = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f);
 		_baro_b_est.setMaxStateNoise(_params.baro_noise);
 		_baro_b_est.setProcessNoiseStdDev(_params.baro_drift_rate);

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -44,6 +44,19 @@
 
 #include <mathlib/mathlib.h>
 
+EstimatorInterface::~EstimatorInterface()
+{
+	delete _gps_buffer;
+	delete _mag_buffer;
+	delete _baro_buffer;
+	delete _range_buffer;
+	delete _airspeed_buffer;
+	delete _flow_buffer;
+	delete _ext_vision_buffer;
+	delete _drag_buffer;
+	delete _auxvel_buffer;
+}
+
 // Accumulate imu data and store to buffer at desired rate
 void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 {
@@ -104,70 +117,59 @@ bool EstimatorInterface::checkIfVehicleAtRest(float dt, const imuSample &imu)
 
 void EstimatorInterface::setMagData(const magSample &mag_sample)
 {
-	if (!_initialised || _mag_buffer_fail) {
+	if (!_initialised) {
 		return;
 	}
 
 	// Allocate the required buffer size if not previously done
-	// Do not retry if allocation has failed previously
-	if (_mag_buffer.get_length() < _obs_buffer_length) {
-		_mag_buffer_fail = !_mag_buffer.allocate(_obs_buffer_length);
+	if (_mag_buffer == nullptr) {
+		_mag_buffer = new RingBuffer<magSample>(_obs_buffer_length);
 
-		if (_mag_buffer_fail) {
+		if (_mag_buffer == nullptr || !_mag_buffer->valid()) {
+			delete _mag_buffer;
+			_mag_buffer = nullptr;
 			printBufferAllocationFailed("mag");
 			return;
 		}
 	}
 
-	// downsample to highest possible sensor rate
-	// by taking the average of incoming sample
-	_mag_sample_count++;
-	_mag_data_sum += mag_sample.mag;
-	_mag_timestamp_sum += mag_sample.time_us / 1000; // Dividing by 1000 to avoid overflow
-
 	// limit data rate to prevent data being lost
 	if ((mag_sample.time_us - _time_last_mag) > _min_obs_interval_us) {
 		_time_last_mag = mag_sample.time_us;
 
 		magSample mag_sample_new;
 
-		// Use the time in the middle of the downsampling interval for the sample
-		mag_sample_new.time_us = 1000 * (_mag_timestamp_sum / _mag_sample_count);
+		mag_sample_new.time_us = mag_sample.time_us;
 		mag_sample_new.time_us -= static_cast<uint64_t>(_params.mag_delay_ms * 1000);
 		mag_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
 
-		mag_sample_new.mag = _mag_data_sum / _mag_sample_count;
+		mag_sample_new.mag = mag_sample.mag;
 
-		_mag_buffer.push(mag_sample_new);
-
-		_mag_sample_count = 0;
-		_mag_data_sum.setZero();
-		_mag_timestamp_sum = 0;
+		_mag_buffer->push(mag_sample_new);
+	} else {
+		ECL_ERR("mag data too fast %" PRIu64, mag_sample.time_us - _time_last_mag);
 	}
 }
 
 void EstimatorInterface::setGpsData(const gps_message &gps)
 {
-	if (!_initialised || _gps_buffer_fail) {
+	if (!_initialised) {
 		return;
 	}
 
 	// Allocate the required buffer size if not previously done
-	// Do not retry if allocation has failed previously
-	if (_gps_buffer.get_length() < _obs_buffer_length) {
-		_gps_buffer_fail = !_gps_buffer.allocate(_obs_buffer_length);
+	if (_gps_buffer == nullptr) {
+		_gps_buffer = new RingBuffer<gpsSample>(_obs_buffer_length);
 
-		if (_gps_buffer_fail) {
+		if (_gps_buffer == nullptr || !_gps_buffer->valid()) {
+			delete _gps_buffer;
+			_gps_buffer = nullptr;
 			printBufferAllocationFailed("GPS");
 			return;
 		}
 	}
 
-	// limit data rate to prevent data being lost
-	const bool need_gps = (_params.fusion_mode & MASK_USE_GPS) || (_params.vdist_sensor_type == VDIST_SENSOR_GPS);
-
-	// TODO: remove checks that are not timing related
-	if (((gps.time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps && gps.fix_type > 2) {
+	if ((gps.time_usec - _time_last_gps) > _min_obs_interval_us) {
 		_time_last_gps = gps.time_usec;
 
 		gpsSample gps_sample_new;
@@ -202,67 +204,60 @@ void EstimatorInterface::setGpsData(const gps_message &gps)
 			gps_sample_new.pos(1) = 0.0f;
 		}
 
-		_gps_buffer.push(gps_sample_new);
+		_gps_buffer->push(gps_sample_new);
+	} else {
+		ECL_ERR("GPS data too fast %" PRIu64, gps.time_usec - _time_last_gps);
 	}
 }
 
 void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 {
-	if (!_initialised || _baro_buffer_fail) {
+	if (!_initialised) {
 		return;
 	}
 
 	// Allocate the required buffer size if not previously done
-	// Do not retry if allocation has failed previously
-	if (_baro_buffer.get_length() < _obs_buffer_length) {
-		_baro_buffer_fail = !_baro_buffer.allocate(_obs_buffer_length);
+	if (_baro_buffer == nullptr) {
+		_baro_buffer = new RingBuffer<baroSample>(_obs_buffer_length);
 
-		if (_baro_buffer_fail) {
+		if (_baro_buffer == nullptr || !_baro_buffer->valid()) {
+			delete _baro_buffer;
+			_baro_buffer = nullptr;
 			printBufferAllocationFailed("baro");
 			return;
 		}
 	}
 
-	// downsample to highest possible sensor rate
-	// by baro data by taking the average of incoming sample
-	_baro_sample_count++;
-	_baro_alt_sum += baro_sample.hgt;
-	_baro_timestamp_sum += baro_sample.time_us / 1000; // Dividing by 1000 to avoid overflow
-
 	// limit data rate to prevent data being lost
 	if ((baro_sample.time_us - _time_last_baro) > _min_obs_interval_us) {
 		_time_last_baro = baro_sample.time_us;
 
-		const float baro_alt_avg = _baro_alt_sum / (float)_baro_sample_count;
-
 		baroSample baro_sample_new;
-		baro_sample_new.hgt = compensateBaroForDynamicPressure(baro_alt_avg);
+		baro_sample_new.hgt = compensateBaroForDynamicPressure(baro_sample.hgt);
 
-		// Use the time in the middle of the downsampling interval for the sample
-		baro_sample_new.time_us = 1000 * (_baro_timestamp_sum / _baro_sample_count);
+		baro_sample_new.time_us = baro_sample.time_us;
 		baro_sample_new.time_us -= static_cast<uint64_t>(_params.baro_delay_ms * 1000);
 		baro_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
 
-		_baro_buffer.push(baro_sample_new);
-
-		_baro_sample_count = 0;
-		_baro_alt_sum = 0.0f;
-		_baro_timestamp_sum = 0;
+		_baro_buffer->push(baro_sample_new);
+	} else {
+		ECL_ERR("baro data too fast %" PRIu64, baro_sample.time_us - _time_last_baro);
 	}
 }
 
 void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 {
-	if (!_initialised || _airspeed_buffer_fail) {
+	if (!_initialised) {
 		return;
 	}
 
 	// Allocate the required buffer size if not previously done
-	// Do not retry if allocation has failed previously
-	if (_airspeed_buffer.get_length() < _obs_buffer_length) {
-		_airspeed_buffer_fail = !_airspeed_buffer.allocate(_obs_buffer_length);
+	if (_airspeed_buffer == nullptr) {
+		_airspeed_buffer = new RingBuffer<airspeedSample>(_obs_buffer_length);
 
-		if (_airspeed_buffer_fail) {
+		if (_airspeed_buffer == nullptr || !_airspeed_buffer->valid()) {
+			delete _airspeed_buffer;
+			_airspeed_buffer = nullptr;
 			printBufferAllocationFailed("airspeed");
 			return;
 		}
@@ -277,22 +272,23 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 		airspeed_sample_new.time_us -= static_cast<uint64_t>(_params.airspeed_delay_ms * 1000);
 		airspeed_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
 
-		_airspeed_buffer.push(airspeed_sample_new);
+		_airspeed_buffer->push(airspeed_sample_new);
 	}
 }
 
 void EstimatorInterface::setRangeData(const rangeSample &range_sample)
 {
-	if (!_initialised || _range_buffer_fail) {
+	if (!_initialised) {
 		return;
 	}
 
 	// Allocate the required buffer size if not previously done
-	// Do not retry if allocation has failed previously
-	if (_range_buffer.get_length() < _obs_buffer_length) {
-		_range_buffer_fail = !_range_buffer.allocate(_obs_buffer_length);
+	if (_range_buffer == nullptr) {
+		_range_buffer = new RingBuffer<rangeSample>(_obs_buffer_length);
 
-		if (_range_buffer_fail) {
+		if (_range_buffer == nullptr || !_range_buffer->valid()) {
+			delete _range_buffer;
+			_range_buffer = nullptr;
 			printBufferAllocationFailed("range");
 			return;
 		}
@@ -306,22 +302,23 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample)
 		range_sample_new.time_us -= static_cast<uint64_t>(_params.range_delay_ms * 1000);
 		range_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
 
-		_range_buffer.push(range_sample_new);
+		_range_buffer->push(range_sample_new);
 	}
 }
 
 void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
 {
-	if (!_initialised || _flow_buffer_fail) {
+	if (!_initialised) {
 		return;
 	}
 
 	// Allocate the required buffer size if not previously done
-	// Do not retry if allocation has failed previously
-	if (_flow_buffer.get_length() < _imu_buffer_length) {
-		_flow_buffer_fail = !_flow_buffer.allocate(_imu_buffer_length);
+	if (_flow_buffer == nullptr) {
+		_flow_buffer = new RingBuffer<flowSample>(_imu_buffer_length);
 
-		if (_flow_buffer_fail) {
+		if (_flow_buffer == nullptr || !_flow_buffer->valid()) {
+			delete _flow_buffer;
+			_flow_buffer = nullptr;
 			printBufferAllocationFailed("flow");
 			return;
 		}
@@ -336,23 +333,24 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
 		optflow_sample_new.time_us -= static_cast<uint64_t>(_params.flow_delay_ms * 1000);
 		optflow_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
 
-		_flow_buffer.push(optflow_sample_new);
+		_flow_buffer->push(optflow_sample_new);
 	}
 }
 
 // set attitude and position data derived from an external vision system
 void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
 {
-	if (!_initialised || _ev_buffer_fail) {
+	if (!_initialised) {
 		return;
 	}
 
 	// Allocate the required buffer size if not previously done
-	// Do not retry if allocation has failed previously
-	if (_ext_vision_buffer.get_length() < _obs_buffer_length) {
-		_ev_buffer_fail = !_ext_vision_buffer.allocate(_obs_buffer_length);
+	if (_ext_vision_buffer == nullptr) {
+		_ext_vision_buffer = new RingBuffer<extVisionSample>(_obs_buffer_length);
 
-		if (_ev_buffer_fail) {
+		if (_ext_vision_buffer == nullptr || !_ext_vision_buffer->valid()) {
+			delete _ext_vision_buffer;
+			_ext_vision_buffer = nullptr;
 			printBufferAllocationFailed("vision");
 			return;
 		}
@@ -367,22 +365,23 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
 		ev_sample_new.time_us -= static_cast<uint64_t>(_params.ev_delay_ms * 1000);
 		ev_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
 
-		_ext_vision_buffer.push(ev_sample_new);
+		_ext_vision_buffer->push(ev_sample_new);
 	}
 }
 
 void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
 {
-	if (!_initialised || _auxvel_buffer_fail) {
+	if (!_initialised) {
 		return;
 	}
 
 	// Allocate the required buffer size if not previously done
-	// Do not retry if allocation has failed previously
-	if (_auxvel_buffer.get_length() < _obs_buffer_length) {
-		_auxvel_buffer_fail = !_auxvel_buffer.allocate(_obs_buffer_length);
+	if (_auxvel_buffer == nullptr) {
+		_auxvel_buffer = new RingBuffer<auxVelSample>(_obs_buffer_length);
 
-		if (_auxvel_buffer_fail) {
+		if (_auxvel_buffer == nullptr || !_auxvel_buffer->valid()) {
+			delete _auxvel_buffer;
+			_auxvel_buffer = nullptr;
 			printBufferAllocationFailed("aux vel");
 			return;
 		}
@@ -397,7 +396,7 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
 		auxvel_sample_new.time_us -= static_cast<uint64_t>(_params.auxvel_delay_ms * 1000);
 		auxvel_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
 
-		_auxvel_buffer.push(auxvel_sample_new);
+		_auxvel_buffer->push(auxvel_sample_new);
 	}
 }
 
@@ -405,14 +404,15 @@ void EstimatorInterface::setDragData(const imuSample &imu)
 {
 	// down-sample the drag specific force data by accumulating and calculating the mean when
 	// sufficient samples have been collected
-	if ((_params.fusion_mode & MASK_USE_DRAG) && !_drag_buffer_fail) {
+	if ((_params.fusion_mode & MASK_USE_DRAG)) {
 
 		// Allocate the required buffer size if not previously done
-		// Do not retry if allocation has failed previously
-		if (_drag_buffer.get_length() < _obs_buffer_length) {
-			_drag_buffer_fail = !_drag_buffer.allocate(_obs_buffer_length);
+		if (_drag_buffer == nullptr) {
+			_drag_buffer = new RingBuffer<dragSample>(_obs_buffer_length);
 
-			if (_drag_buffer_fail) {
+			if (_drag_buffer == nullptr || !_drag_buffer->valid()) {
+				delete _drag_buffer;
+				_drag_buffer = nullptr;
 				printBufferAllocationFailed("drag");
 				return;
 			}
@@ -440,7 +440,7 @@ void EstimatorInterface::setDragData(const imuSample &imu)
 			_drag_down_sampled.time_us /= _drag_sample_count;
 
 			// write to buffer
-			_drag_buffer.push(_drag_down_sampled);
+			_drag_buffer->push(_drag_down_sampled);
 
 			// reset accumulators
 			_drag_sample_count = 0;
@@ -454,9 +454,14 @@ void EstimatorInterface::setDragData(const imuSample &imu)
 bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 {
 	// find the maximum time delay the buffers are required to handle
-	// it's reasonable to assume that barometer is always used, and its delay is low
+
 	// it's reasonable to assume that aux velocity device has low delay. TODO: check the delay only if the aux device is used
-	float max_time_delay_ms = math::max(_params.baro_delay_ms, _params.auxvel_delay_ms);
+	float max_time_delay_ms = math::max((float)_params.sensor_interval_max_ms, _params.auxvel_delay_ms);
+
+	// using baro
+	if (_params.vdist_sensor_type == 0) {
+		max_time_delay_ms = math::max(_params.baro_delay_ms, max_time_delay_ms);
+	}
 
 	// using airspeed
 	if (_params.arsp_thr > FLT_EPSILON) {
@@ -485,18 +490,20 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 		max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms);
 	}
 
-	// calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter
-	_imu_buffer_length = ceilf(max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1;
+	// calculate the IMU buffer length required to accommodate the maximum delay with some allowance for jitter
+	_imu_buffer_length = ceilf(max_time_delay_ms / FILTER_UPDATE_PERIOD_MS);
 
 	// set the observation buffer length to handle the minimum time of arrival between observations in combination
 	// with the worst case delay from current time to ekf fusion time
 	// allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter
 	const float ekf_delay_ms = max_time_delay_ms * 1.5f;
-	_obs_buffer_length = ceilf(ekf_delay_ms / _params.sensor_interval_min_ms);
+	_obs_buffer_length = roundf(ekf_delay_ms / FILTER_UPDATE_PERIOD_MS);
 
 	// limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)
 	_obs_buffer_length = math::min(_obs_buffer_length, _imu_buffer_length);
 
+	ECL_DEBUG("EKF max time delay %.1f ms, OBS length %d\n", (double)ekf_delay_ms, _obs_buffer_length);
+
 	if (!_imu_buffer.allocate(_imu_buffer_length) || !_output_buffer.allocate(_imu_buffer_length)
 	    || !_output_vert_buffer.allocate(_imu_buffer_length)) {
 
@@ -550,15 +557,43 @@ void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name)
 
 void EstimatorInterface::print_status()
 {
-	ECL_INFO("imu buffer: %d (%d Bytes)", _imu_buffer.get_length(), _imu_buffer.get_total_size());
-	ECL_INFO("gps buffer: %d (%d Bytes)", _gps_buffer.get_length(), _gps_buffer.get_total_size());
-	ECL_INFO("mag buffer: %d (%d Bytes)", _mag_buffer.get_length(), _mag_buffer.get_total_size());
-	ECL_INFO("baro buffer: %d (%d Bytes)", _baro_buffer.get_length(), _baro_buffer.get_total_size());
-	ECL_INFO("range buffer: %d (%d Bytes)", _range_buffer.get_length(), _range_buffer.get_total_size());
-	ECL_INFO("airspeed buffer: %d (%d Bytes)", _airspeed_buffer.get_length(), _airspeed_buffer.get_total_size());
-	ECL_INFO("flow buffer: %d (%d Bytes)", _flow_buffer.get_length(), _flow_buffer.get_total_size());
-	ECL_INFO("vision buffer: %d (%d Bytes)", _ext_vision_buffer.get_length(), _ext_vision_buffer.get_total_size());
-	ECL_INFO("output buffer: %d (%d Bytes)", _output_buffer.get_length(), _output_buffer.get_total_size());
-	ECL_INFO("output vert buffer: %d (%d Bytes)", _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size());
-	ECL_INFO("drag buffer: %d (%d Bytes)", _drag_buffer.get_length(), _drag_buffer.get_total_size());
+	printf("IMU average dt: %.6f seconds\n", (double)_dt_imu_avg);
+	printf("IMU buffer: %d (%d Bytes)\n", _imu_buffer.get_length(), _imu_buffer.get_total_size());
+
+	printf("minimum observation interval %d us\n", _min_obs_interval_us);
+
+	if (_gps_buffer) {
+		printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size());
+	}
+
+	if (_mag_buffer) {
+		printf("mag buffer: %d/%d (%d Bytes)\n", _mag_buffer->entries(), _mag_buffer->get_length(), _mag_buffer->get_total_size());
+	}
+
+	if (_baro_buffer) {
+		printf("baro buffer: %d/%d (%d Bytes)\n", _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size());
+	}
+
+	if (_range_buffer) {
+		printf("range buffer: %d/%d (%d Bytes)\n", _range_buffer->entries(), _range_buffer->get_length(), _range_buffer->get_total_size());
+	}
+
+	if (_airspeed_buffer) {
+		printf("airspeed buffer: %d/%d (%d Bytes)\n", _airspeed_buffer->entries(), _airspeed_buffer->get_length(), _airspeed_buffer->get_total_size());
+	}
+
+	if (_flow_buffer) {
+		printf("flow buffer: %d/%d (%d Bytes)\n", _flow_buffer->entries(), _flow_buffer->get_length(), _flow_buffer->get_total_size());
+	}
+
+	if (_ext_vision_buffer) {
+		printf("vision buffer: %d/%d (%d Bytes)\n", _ext_vision_buffer->entries(), _ext_vision_buffer->get_length(), _ext_vision_buffer->get_total_size());
+	}
+
+	if (_drag_buffer) {
+		printf("drag buffer: %d/%d (%d Bytes)\n", _drag_buffer->entries(), _drag_buffer->get_length(), _drag_buffer->get_total_size());
+	}
+
+	printf("output buffer: %d/%d (%d Bytes)\n", _output_buffer.entries(), _output_buffer.get_length(), _output_buffer.get_total_size());
+	printf("output vert buffer: %d/%d (%d Bytes)\n", _output_vert_buffer.entries(), _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size());
 }

src/modules/ekf2/EKF/estimator_interface.h

@@ -46,11 +46,18 @@
 # define ECL_INFO PX4_DEBUG
 # define ECL_WARN PX4_DEBUG
 # define ECL_ERR  PX4_DEBUG
+# define ECL_DEBUG PX4_DEBUG
 #else
 # define ECL_INFO(X, ...) printf(X "\n", ##__VA_ARGS__)
 # define ECL_WARN(X, ...) fprintf(stderr, X "\n", ##__VA_ARGS__)
 # define ECL_ERR(X, ...) fprintf(stderr, X "\n", ##__VA_ARGS__)
 
+# if defined(DEBUG_BUILD)
+#  define ECL_DEBUG(X, ...) fprintf(stderr, X "\n", ##__VA_ARGS__)
+# else
+#  define ECL_DEBUG(X, ...)
+#endif
+
 #endif
 
 #include "common.h"
@@ -263,7 +270,7 @@ public:
 protected:
 
 	EstimatorInterface() = default;
-	virtual ~EstimatorInterface() = default;
+	virtual ~EstimatorInterface();
 
 	virtual bool init(uint64_t timestamp) = 0;
 
@@ -365,15 +372,15 @@ protected:
 	RingBuffer<outputSample> _output_buffer{12};
 	RingBuffer<outputVert> _output_vert_buffer{12};
 
-	RingBuffer<gpsSample> _gps_buffer;
-	RingBuffer<magSample> _mag_buffer;
-	RingBuffer<baroSample> _baro_buffer;
-	RingBuffer<rangeSample> _range_buffer;
-	RingBuffer<airspeedSample> _airspeed_buffer;
-	RingBuffer<flowSample> 	_flow_buffer;
-	RingBuffer<extVisionSample> _ext_vision_buffer;
-	RingBuffer<dragSample> _drag_buffer;
-	RingBuffer<auxVelSample> _auxvel_buffer;
+	RingBuffer<gpsSample> *_gps_buffer{nullptr};
+	RingBuffer<magSample> *_mag_buffer{nullptr};
+	RingBuffer<baroSample> *_baro_buffer{nullptr};
+	RingBuffer<rangeSample> *_range_buffer{nullptr};
+	RingBuffer<airspeedSample> *_airspeed_buffer{nullptr};
+	RingBuffer<flowSample> 	*_flow_buffer{nullptr};
+	RingBuffer<extVisionSample> *_ext_vision_buffer{nullptr};
+	RingBuffer<dragSample> *_drag_buffer{nullptr};
+	RingBuffer<auxVelSample> *_auxvel_buffer{nullptr};
 
 	// timestamps of latest in buffer saved measurement in microseconds
 	uint64_t _time_last_imu{0};
@@ -429,30 +436,11 @@ private:
 					// [1] high frequency vibration level in the IMU delta angle data (rad)
 					// [2] high frequency vibration level in the IMU delta velocity data (m/s)
 
-	// Used to down sample barometer data
-	uint64_t _baro_timestamp_sum{0};	// summed timestamp to provide the timestamp of the averaged sample
-	float _baro_alt_sum{0.0f};			// summed pressure altitude readings (m)
-	uint8_t _baro_sample_count{0};		// number of barometric altitude measurements summed
-
 	// Used by the multi-rotor specific drag force fusion
 	uint8_t _drag_sample_count{0};	// number of drag specific force samples assumulated at the filter prediction rate
 	float _drag_sample_time_dt{0.0f};	// time integral across all samples used to form _drag_down_sampled (sec)
 
-	// Used to downsample magnetometer data
-	uint64_t _mag_timestamp_sum{0};
-	Vector3f _mag_data_sum{};
-	uint8_t _mag_sample_count{0};
-
 	// observation buffer final allocation failed
-	bool _gps_buffer_fail{false};
-	bool _mag_buffer_fail{false};
-	bool _baro_buffer_fail{false};
-	bool _range_buffer_fail{false};
-	bool _airspeed_buffer_fail{false};
-	bool _flow_buffer_fail{false};
-	bool _ev_buffer_fail{false};
-	bool _drag_buffer_fail{false};
-	bool _auxvel_buffer_fail{false};
-
+	uint64_t _last_allocation_fail_print{0};
 };
 #endif // !EKF_ESTIMATOR_INTERFACE_H

src/modules/ekf2/EKF2.cpp

@@ -57,7 +57,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_odometry_pub(multi_mode ? ORB_ID(estimator_odometry) : ORB_ID(vehicle_odometry)),
 	_wind_pub(multi_mode ? ORB_ID(estimator_wind) : ORB_ID(wind)),
 	_params(_ekf.getParamHandle()),
-	_param_ekf2_min_obs_dt(_params->sensor_interval_min_ms),
 	_param_ekf2_mag_delay(_params->mag_delay_ms),
 	_param_ekf2_baro_delay(_params->baro_delay_ms),
 	_param_ekf2_gps_delay(_params->gps_delay_ms),
@@ -243,6 +242,10 @@ int EKF2::print_status()
 	perf_print_counter(_msg_missed_odometry_perf);
 	perf_print_counter(_msg_missed_optical_flow_perf);
 
+#if defined(DEBUG_BUILD)
+	_ekf.print_status();
+#endif // DEBUG_BUILD
+
 	return 0;
 }
 
@@ -272,6 +275,38 @@ void EKF2::Run()
 		if (param_aspd_scale != PARAM_INVALID) {
 			param_get(param_aspd_scale, &_airspeed_scale_factor);
 		}
+
+		// if using baro ensure sensor interval minimum is sufficient to accommodate system averaged baro output
+		if (_params->vdist_sensor_type == 0) {
+			float sens_baro_rate = 0.f;
+
+			if (param_get(param_find("SENS_BARO_RATE"), &sens_baro_rate) == PX4_OK) {
+				if (sens_baro_rate > 0) {
+					float interval_ms = roundf(1000.f / sens_baro_rate);
+
+					if (PX4_ISFINITE(interval_ms) && (interval_ms > _params->sensor_interval_max_ms)) {
+						PX4_DEBUG("updating sensor_interval_max_ms %.3f -> %.3f", (double)_params->sensor_interval_max_ms, (double)interval_ms);
+						_params->sensor_interval_max_ms = interval_ms;
+					}
+				}
+			}
+		}
+
+		// if using mag ensure sensor interval minimum is sufficient to accommodate system averaged mag output
+		if (_params->mag_fusion_type != MAG_FUSE_TYPE_NONE) {
+			float sens_mag_rate = 0.f;
+
+			if (param_get(param_find("SENS_MAG_RATE"), &sens_mag_rate) == PX4_OK) {
+				if (sens_mag_rate > 0) {
+					float interval_ms = roundf(1000.f / sens_mag_rate);
+
+					if (PX4_ISFINITE(interval_ms) && (interval_ms > _params->sensor_interval_max_ms)) {
+						PX4_DEBUG("updating sensor_interval_max_ms %.3f -> %.3f", (double)_params->sensor_interval_max_ms, (double)interval_ms);
+						_params->sensor_interval_max_ms = interval_ms;
+					}
+				}
+			}
+		}
 	}
 
 	if (!_callback_registered) {

src/modules/ekf2/EKF2.hpp

@@ -315,8 +315,6 @@ private:
 	parameters *_params;	///< pointer to ekf parameter struct (located in _ekf class instance)
 
 	DEFINE_PARAMETERS(
-		(ParamExtInt<px4::params::EKF2_MIN_OBS_DT>)
-		_param_ekf2_min_obs_dt,	///< Maximum time delay of any sensor used to increase buffer length to handle large timing jitter (mSec)
 		(ParamExtFloat<px4::params::EKF2_MAG_DELAY>)
 		_param_ekf2_mag_delay,	///< magnetometer measurement delay relative to the IMU (mSec)
 		(ParamExtFloat<px4::params::EKF2_BARO_DELAY>)

src/modules/ekf2/ekf2_params.c

@@ -39,19 +39,6 @@
  *
  */
 
-/**
- * Minimum time of arrival delta between non-IMU observations before data is downsampled.
- *
- * Baro and Magnetometer data will be averaged before downsampling, other data will be point sampled resulting in loss of information.
- *
- * @group EKF2
- * @min 10
- * @max 50
- * @reboot_required true
- * @unit ms
- */
-PARAM_DEFINE_INT32(EKF2_MIN_OBS_DT, 20);
-
 /**
  * Magnetometer measurement delay relative to IMU measurements
  *

src/modules/ekf2/test/test_EKF_ringbuffer.cpp

@@ -46,11 +46,11 @@ class EkfRingBufferTest : public ::testing::Test
 public:
 
 	sample _x, _y, _z;
-	RingBuffer<sample> *_buffer;
+	RingBuffer<sample> *_buffer{nullptr};
 
 	void SetUp() override
 	{
-		_buffer = new RingBuffer<sample>();
+		_buffer = new RingBuffer<sample>(3);
 		_x.time_us = 1000000;
 		_x.data[0] = _x.data[1] = _x.data[2] = 1.5f;