ekf2: improve ring buffer sizing and dynamically allocate

2026-07-13 06:40:34 +08:00 · 2022-01-13 17:42:36 -05:00
parent 36605bfff6
commit 92a5bbe97f
13 changed files with 356 additions and 247 deletions
@@ -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());
 }