Merge pull request #360 from dagar/pr-ekfObsBufAlloc

EKF RingBuffer allocation minor improvements + print memory usage

EKF/RingBuffer.h

@@ -45,7 +45,13 @@ template <typename data_type>
 class RingBuffer
 {
 public:
-	RingBuffer() = default;
+	RingBuffer() {
+		if (allocate(1)) {
+			// initialize with one empty sample
+			data_type d = {};
+			push(d);
+		}
+	}
 	~RingBuffer() { delete[] _buffer; }
 
 	// no copy, assignment, move, move assignment
@@ -72,8 +78,8 @@ public:
 		_tail = 0;
 
 		// set the time elements to zero so that bad data is not retrieved from the buffers
-		for (unsigned index = 0; index < _size; index++) {
-			_buffer[index].time_us = 0;
+		for (uint8_t index = 0; index < _size; index++) {
+			_buffer[index] = {};
 		}
 		_first_write = true;
 
@@ -149,13 +155,8 @@ public:
 		return false;
 	}
 
+	int get_total_size() { return sizeof(*this) + sizeof(data_type) * _size; }
 
-
-	// return ttrue if allocated
-	unsigned is_allocated()
-	{
-		return (_buffer != NULL);
-	}
 private:
 	data_type *_buffer{nullptr};
 

EKF/estimator_interface.cpp

@@ -94,19 +94,27 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
 		_imu_ticks = 0;
 		_imu_updated = true;
 
-		// Allocate the required buffer size if not previously done
-		// Do not retry if allocation has failed previously
-		if (!_drag_buffer_pass && !_drag_buffer_fail) {
-			_drag_buffer_pass = _drag_buffer.allocate(_obs_buffer_length);
-			_drag_buffer_fail = !_drag_buffer_pass;
-			if (_drag_buffer_fail) {
-				ECL_ERR("EKF drag buffer allocation failed");
-			}
-		}
+		// get the oldest data from the buffer
+		_imu_sample_delayed = _imu_buffer.get_oldest();
+
+		// calculate the minimum interval between observations required to guarantee no loss of data
+		// this will occur if data is overwritten before its time stamp falls behind the fusion time horizon
+		_min_obs_interval_us = (_imu_sample_new.time_us - _imu_sample_delayed.time_us) / (_obs_buffer_length - 1);
 
 		// 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_pass) {
+		if ((_params.fusion_mode & MASK_USE_DRAG) && !_drag_buffer_fail) {
+
+			// 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_fail) {
+					ECL_ERR("EKF drag buffer allocation failed");
+					return;
+				}
+			}
+
 			_drag_sample_count ++;
 			// note acceleration is accumulated as a delta velocity
 			_drag_down_sampled.accelXY(0) += imu_sample_new.delta_vel(0);
@@ -119,7 +127,6 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
 
 			if (min_sample_ratio < 5) {
 				min_sample_ratio = 5;
-
 			}
 
 			// calculate and store means from accumulated values
@@ -129,6 +136,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
 				_drag_down_sampled.accelXY(1) /= _drag_sample_time_dt;
 				_drag_down_sampled.time_us /= _drag_sample_count;
 
+				// write to buffer
 				_drag_buffer.push(_drag_down_sampled);
 
 				// reset accumulators
@@ -140,13 +148,6 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
 			}
 		}
 
-		// get the oldest data from the buffer
-		_imu_sample_delayed = _imu_buffer.get_oldest();
-
-		// calculate the minimum interval between observations required to guarantee no loss of data
-		// this will occur if data is overwritten before its time stamp falls behind the fusion time horizon
-		_min_obs_interval_us = (_imu_sample_new.time_us - _imu_sample_delayed.time_us) / (_obs_buffer_length - 1);
-
 	} else {
 		_imu_updated = false;
 
@@ -155,22 +156,22 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
 
 void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3])
 {
-	if (!_initialised) {
+	if (!_initialised || _mag_buffer_fail) {
 		return;
 	}
 
 	// Allocate the required buffer size if not previously done
 	// Do not retry if allocation has failed previously
-	if (!_mag_buffer_pass && !_mag_buffer_fail) {
-		_mag_buffer_pass = _mag_buffer.allocate(_obs_buffer_length);
-		_mag_buffer_fail = !_mag_buffer_pass;
+	if (_mag_buffer.get_length() < _obs_buffer_length) {
+		_mag_buffer_fail = !_mag_buffer.allocate(_obs_buffer_length);
 		if (_mag_buffer_fail) {
 			ECL_ERR("EKF mag buffer allocation failed");
+			return;
 		}
 	}
 
 	// limit data rate to prevent data being lost
-	if ((time_usec - _time_last_mag > _min_obs_interval_us) && _mag_buffer_pass) {
+	if (time_usec - _time_last_mag > _min_obs_interval_us) {
 
 		magSample mag_sample_new;
 		mag_sample_new.time_us = time_usec - _params.mag_delay_ms * 1000;
@@ -181,30 +182,25 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3])
 		mag_sample_new.mag = Vector3f(data);
 
 		_mag_buffer.push(mag_sample_new);
-
 	}
 }
 
 void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
 {
-	if (!_initialised) {
+	if (!_initialised || _gps_buffer_fail) {
 		return;
 	}
 
 	// Allocate the required buffer size if not previously done
 	// Do not retry if allocation has failed previously
-	if (!_gps_buffer_pass && !_gps_buffer_fail) {
-		_gps_buffer_pass = _gps_buffer.allocate(_obs_buffer_length);
-		_gps_buffer_fail = !_gps_buffer_pass;
+	if (_gps_buffer.get_length() < _obs_buffer_length) {
+		_gps_buffer_fail = !_gps_buffer.allocate(_obs_buffer_length);
 		if (_gps_buffer_fail) {
 			ECL_ERR("EKF GPS buffer allocation failed");
+			return;
 		}
 	}
 
-	if (!_gps_buffer_pass) {
-		return;
-	}
-
 	// limit data rate to prevent data being lost
 	bool need_gps = (_params.fusion_mode & MASK_USE_GPS) || (_params.vdist_sensor_type == VDIST_SENSOR_GPS);
 
@@ -236,7 +232,6 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
 		} else {
 			gps_sample_new.pos(0) = 0.0f;
 			gps_sample_new.pos(1) = 0.0f;
-
 		}
 
 		_gps_buffer.push(gps_sample_new);
@@ -245,22 +240,22 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
 
 void EstimatorInterface::setBaroData(uint64_t time_usec, float data)
 {
-	if (!_initialised) {
+	if (!_initialised || _baro_buffer_fail) {
 		return;
 	}
 
 	// Allocate the required buffer size if not previously done
 	// Do not retry if allocation has failed previously
-	if (!_baro_buffer_pass && !_baro_buffer_fail) {
-		_baro_buffer_pass = _baro_buffer.allocate(_obs_buffer_length);
-		_baro_buffer_fail = !_baro_buffer_pass;
+	if (_baro_buffer.get_length() < _obs_buffer_length) {
+		_baro_buffer_fail = !_baro_buffer.allocate(_obs_buffer_length);
 		if (_baro_buffer_fail) {
 			ECL_ERR("EKF baro buffer allocation failed");
+			return;
 		}
 	}
 
 	// limit data rate to prevent data being lost
-	if ((time_usec - _time_last_baro > _min_obs_interval_us) && _baro_buffer_pass) {
+	if (time_usec - _time_last_baro > _min_obs_interval_us) {
 
 		baroSample baro_sample_new;
 		baro_sample_new.hgt = data;
@@ -277,22 +272,22 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float data)
 
 void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed, float eas2tas)
 {
-	if (!_initialised) {
+	if (!_initialised || _airspeed_buffer_fail) {
 		return;
 	}
 
 	// Allocate the required buffer size if not previously done
 	// Do not retry if allocation has failed previously
-	if (!_airspeed_buffer_pass && !_airspeed_buffer_fail) {
-		_airspeed_buffer_pass = _airspeed_buffer.allocate(_obs_buffer_length);
-		_airspeed_buffer_fail = !_airspeed_buffer_pass;
+	if (_airspeed_buffer.get_length() < _obs_buffer_length) {
+		_airspeed_buffer_fail = !_airspeed_buffer.allocate(_obs_buffer_length);
 		if (_airspeed_buffer_fail) {
 			ECL_ERR("EKF airspeed buffer allocation failed");
+			return;
 		}
 	}
 
 	// limit data rate to prevent data being lost
-	if ((time_usec - _time_last_airspeed > _min_obs_interval_us) && _airspeed_buffer_pass) {
+	if (time_usec - _time_last_airspeed > _min_obs_interval_us) {
 		airspeedSample airspeed_sample_new;
 		airspeed_sample_new.true_airspeed = true_airspeed;
 		airspeed_sample_new.eas2tas = eas2tas;
@@ -306,22 +301,22 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed
 
 void EstimatorInterface::setRangeData(uint64_t time_usec, float data)
 {
-	if (!_initialised) {
+	if (!_initialised || _range_buffer_fail) {
 		return;
 	}
 
 	// Allocate the required buffer size if not previously done
 	// Do not retry if allocation has failed previously
-	if (!_range_buffer_pass && !_range_buffer_fail) {
-		_range_buffer_pass = _range_buffer.allocate(_obs_buffer_length);
-		_range_buffer_fail = !_range_buffer_pass;
+	if (_range_buffer.get_length() < _obs_buffer_length) {
+		_range_buffer_fail = !_range_buffer.allocate(_obs_buffer_length);
 		if (_range_buffer_fail) {
 			ECL_ERR("EKF range finder buffer allocation failed");
+			return;
 		}
 	}
 
 	// limit data rate to prevent data being lost
-	if ((time_usec - _time_last_range > _min_obs_interval_us) && _range_buffer_pass) {
+	if (time_usec - _time_last_range > _min_obs_interval_us) {
 		rangeSample range_sample_new;
 		range_sample_new.rng = data;
 		range_sample_new.time_us = time_usec - _params.range_delay_ms * 1000;
@@ -334,22 +329,22 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float data)
 // set optical flow data
 void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *flow)
 {
-	if (!_initialised) {
+	if (!_initialised || _flow_buffer_fail) {
 		return;
 	}
 
 	// Allocate the required buffer size if not previously done
 	// Do not retry if allocation has failed previously
-	if (!_flow_buffer_pass && !_flow_buffer_fail) {
-		_flow_buffer_pass = _flow_buffer.allocate(_obs_buffer_length);
-		_flow_buffer_fail = !_flow_buffer_pass;
+	if (_flow_buffer.get_length() < _obs_buffer_length) {
+		_flow_buffer_fail = !_flow_buffer.allocate(_obs_buffer_length);
 		if (_flow_buffer_fail) {
 			ECL_ERR("EKF optical flow buffer allocation failed");
+			return;
 		}
 	}
 
 	// limit data rate to prevent data being lost
-	if ((time_usec - _time_last_optflow > _min_obs_interval_us) && _flow_buffer_pass) {
+	if (time_usec - _time_last_optflow > _min_obs_interval_us) {
 		// check if enough integration time and fail if integration time is less than 50%
 		// of min arrival interval because too much data is being lost
 		float delta_time = 1e-6f * (float)flow->dt;
@@ -413,22 +408,22 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 // set attitude and position data derived from an external vision system
 void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message *evdata)
 {
-	if (!_initialised) {
+	if (!_initialised || _ev_buffer_fail) {
 		return;
 	}
 
 	// Allocate the required buffer size if not previously done
 	// Do not retry if allocation has failed previously
-	if (!_ev_buffer_pass && !_ev_buffer_fail) {
-		_ev_buffer_pass = _ext_vision_buffer.allocate(_obs_buffer_length);
-		_ev_buffer_fail = !_ev_buffer_pass;
+	if (_ext_vision_buffer.get_length() < _obs_buffer_length) {
+		_ev_buffer_fail = !_ext_vision_buffer.allocate(_obs_buffer_length);
 		if (_ev_buffer_fail) {
 			ECL_ERR("EKF external vision buffer allocation failed");
+			return;
 		}
 	}
 
 	// limit data rate to prevent data being lost
-	if ((time_usec - _time_last_ext_vision > _min_obs_interval_us) && _ev_buffer_pass) {
+	if (time_usec - _time_last_ext_vision > _min_obs_interval_us) {
 		extVisionSample ev_sample_new;
 		// calculate the system time-stamp for the mid point of the integration period
 		ev_sample_new.time_us = time_usec - _params.ev_delay_ms * 1000;
@@ -471,49 +466,14 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	_obs_buffer_length = math::min(_obs_buffer_length, _imu_buffer_length);
 
 	if (!(_imu_buffer.allocate(_imu_buffer_length) &&
-	      _gps_buffer.allocate(1) &&
-	      _mag_buffer.allocate(1) &&
-	      _baro_buffer.allocate(1) &&
-	      _range_buffer.allocate(1) &&
-	      _airspeed_buffer.allocate(1) &&
-	      _flow_buffer.allocate(1) &&
-	      _ext_vision_buffer.allocate(1) &&
-	      _drag_buffer.allocate(1) &&
 	      _output_buffer.allocate(_imu_buffer_length) &&
 	      _output_vert_buffer.allocate(_imu_buffer_length))) {
+
 		ECL_ERR("EKF buffer allocation failed!");
 		unallocate_buffers();
 		return false;
 	}
 
-	// zero the data in the observation buffers
-	for (int index = 0; index < _obs_buffer_length; index++) {
-		gpsSample gps_sample_init = {};
-		_gps_buffer.push(gps_sample_init);
-		magSample mag_sample_init = {};
-		_mag_buffer.push(mag_sample_init);
-		baroSample baro_sample_init = {};
-		_baro_buffer.push(baro_sample_init);
-		rangeSample range_sample_init = {};
-		_range_buffer.push(range_sample_init);
-		airspeedSample airspeed_sample_init = {};
-		_airspeed_buffer.push(airspeed_sample_init);
-		flowSample flow_sample_init = {};
-		_flow_buffer.push(flow_sample_init);
-		extVisionSample ext_vision_sample_init = {};
-		_ext_vision_buffer.push(ext_vision_sample_init);
-		dragSample drag_sample_init = {};
-		_drag_buffer.push(drag_sample_init);
-	}
-
-	// zero the data in the imu data and output observer state buffers
-	for (int index = 0; index < _imu_buffer_length; index++) {
-		imuSample imu_sample_init = {};
-		_imu_buffer.push(imu_sample_init);
-		outputSample output_sample_init = {};
-		_output_buffer.push(output_sample_init);
-	}
-
 	_dt_imu_avg = 0.0f;
 
 	_imu_sample_delayed.delta_ang.setZero();
@@ -559,3 +519,20 @@ bool EstimatorInterface::local_position_is_valid()
 	// return true if we are not doing unconstrained free inertial navigation
 	return !inertial_dead_reckoning();
 }
+
+void EstimatorInterface::print_status() {
+	ECL_INFO("local position valid: %s", local_position_is_valid() ? "yes" : "no");
+	ECL_INFO("global position valid: %s", global_position_is_valid() ? "yes" : "no");
+
+	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("ext 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());
+}

EKF/estimator_interface.h

@@ -352,6 +352,8 @@ public:
 		return _imu_updated;
 	}
 
+	void print_status();
+
 	static const unsigned FILTER_UPDATE_PERIOD_MS = 8;	// ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
 
 protected:
@@ -463,16 +465,6 @@ protected:
 	bool _ev_buffer_fail{false};
 	bool _drag_buffer_fail{false};
 
-	// observation buffer final allocation succeeded
-	bool _gps_buffer_pass{false};
-	bool _mag_buffer_pass{false};
-	bool _baro_buffer_pass{false};
-	bool _range_buffer_pass{false};
-	bool _airspeed_buffer_pass{false};
-	bool _flow_buffer_pass{false};
-	bool _ev_buffer_pass{false};
-	bool _drag_buffer_pass{false};
-
 	uint64_t _time_last_imu{0};	// timestamp of last imu sample in microseconds
 	uint64_t _time_last_gps{0};	// timestamp of last gps measurement in microseconds
 	uint64_t _time_last_mag{0};	// timestamp of last magnetometer measurement in microseconds