Merge pull request #358 from PX4/pr-ringbuffer_copies

EKF RingBuffer avoid unnecessary copying

EKF/RingBuffer.h

@@ -45,54 +45,51 @@ template <typename data_type>
 class RingBuffer
 {
 public:
-	RingBuffer()
+	RingBuffer() = default;
-	{
-		_buffer = NULL;
-		_head = _tail = _size = 0;
-		_first_write = true;
-	}
 	~RingBuffer() { delete[] _buffer; }
 
-	bool allocate(int size)
+	// no copy, assignment, move, move assignment
-	{
+	RingBuffer(const RingBuffer &) = delete;
-		if (size <= 0) {
+	RingBuffer &operator=(const RingBuffer &) = delete;
-			return false;
+	RingBuffer(RingBuffer &&) = delete;
-		}
+	RingBuffer &operator=(RingBuffer &&) = delete;
 
-		if (_buffer != NULL) {
+	bool allocate(uint8_t size)
+	{
+		if (_buffer != nullptr) {
 			delete[] _buffer;
 		}
 
 		_buffer = new data_type[size];
 
-		if (_buffer == NULL) {
+		if (_buffer == nullptr) {
 			return false;
 		}
 
 		_size = size;
+
+		_head = 0;
+		_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;
 		}
 		_first_write = true;
+
 		return true;
 	}
 
-	void unallocate()
+	void unallocate() {
-	{
+		delete[] _buffer;
-		if (_buffer != NULL) {
+		_buffer = nullptr;
-			delete[] _buffer;
-		}
 	}
 
-	inline void push(data_type sample)
+	void push(const data_type& sample)
 	{
-		int head_new = _head;
+		uint8_t head_new = _head;
 
-		if (_first_write) {
+		if (!_first_write) {
-			head_new = _head;
-
-		} else {
 			head_new = (_head + 1) % _size;
 		}
 
@@ -108,36 +105,29 @@ public:
 		}
 	}
 
-	inline data_type get_oldest()
+	uint8_t get_length() const { return _size; }
-	{
-		return _buffer[_tail];
-	}
 
-	unsigned get_oldest_index()
+	data_type &operator[](const uint8_t index) { return _buffer[index]; }
-	{
-		return _tail;
-	}
 
-	inline data_type get_newest()
+	const data_type& get_newest() { return _buffer[_head]; }
-	{
+	const data_type& get_oldest() { return _buffer[_tail]; }
-		return _buffer[_head];
-	}
 
-	inline bool pop_first_older_than(uint64_t timestamp, data_type *sample)
+	uint8_t get_oldest_index() const { return _tail; }
+
+	bool pop_first_older_than(const uint64_t& timestamp, data_type *sample)
 	{
 		// start looking from newest observation data
-		for (unsigned i = 0; i < _size; i++) {
+		for (uint8_t i = 0; i < _size; i++) {
 			int index = (_head - i);
 			index = index < 0 ? _size + index : index;
 
 			if (timestamp >= _buffer[index].time_us && timestamp - _buffer[index].time_us < (uint64_t)1e5) {
 
-				// TODO Re-evaluate the static cast and usage patterns
+				*sample = _buffer[index];
-				memcpy(static_cast<void *>(sample), static_cast<void *>(&_buffer[index]), sizeof(*sample));
 
 				// Now we can set the tail to the item which comes after the one we removed
 				// since we don't want to have any older data in the buffer
-				if (index == static_cast<int>(_head)) {
+				if (index == _head) {
 					_tail = _head;
 					_first_write = true;
 
@@ -150,7 +140,7 @@ public:
 				return true;
 			}
 
-			if (index == static_cast<int>(_tail)) {
+			if (index == _tail) {
 				// we have reached the tail and haven't got a match
 				return false;
 			}
@@ -159,38 +149,14 @@ public:
 		return false;
 	}
 
-	data_type &operator[](unsigned index)
-	{
-		return _buffer[index];
-	}
 
-	// return data at the specified index
-	inline data_type get_from_index(unsigned index)
-	{
-		if (index >= _size) {
-			index = _size-1;
-		}
-		return _buffer[index];
-	}
-
-	// push data to the specified index
-	inline void push_to_index(unsigned index, data_type sample)
-	{
-		if (index >= _size) {
-			index = _size-1;
-		}
-		_buffer[index] = sample;
-	}
-
-	// return the length of the buffer
-	unsigned get_length()
-	{
-		return _size;
-	}
 
 private:
-	data_type *_buffer;
+	data_type *_buffer{nullptr};
-	unsigned _head, _tail, _size;
-	bool _first_write;
 
+	uint8_t _head{0};
+	uint8_t _tail{0};
+	uint8_t _size{0};
+
+	bool _first_write{true};
 };

EKF/control.cpp

@@ -80,11 +80,13 @@ void Ekf::controlFusionModes()
 	}
 
 	// check faultiness (before pop_first_older_than) to see if we can change back to original height sensor
-	baroSample baro_init = _baro_buffer.get_newest();
+	const baroSample& baro_init = _baro_buffer.get_newest();
 	_baro_hgt_faulty = !((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
-	gpsSample gps_init = _gps_buffer.get_newest();
+
+	const gpsSample& gps_init = _gps_buffer.get_newest();
 	_gps_hgt_faulty = !((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
-	rangeSample rng_init = _range_buffer.get_newest();
+
+	const rangeSample& rng_init = _range_buffer.get_newest();
 	_rng_hgt_faulty = !((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL);
 
 	// check for arrival of new sensor data at the fusion time horizon
@@ -182,7 +184,7 @@ void Ekf::controlExternalVisionFusion()
 				Eulerf euler_init(q_init);
 
 				// get initial yaw from the observation quaternion
-				extVisionSample ev_newest = _ext_vision_buffer.get_newest();
+				const extVisionSample& ev_newest = _ext_vision_buffer.get_newest();
 				Quatf q_obs(ev_newest.quat);
 				Eulerf euler_obs(q_obs);
 				euler_init(2) = euler_obs(2);
@@ -197,12 +199,9 @@ void Ekf::controlExternalVisionFusion()
 				_state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal;
 
 				// add the reset amount to the output observer buffered data
-				outputSample output_states;
+				// Note q1 *= q2 is equivalent to q1 = q2 * q1
-				unsigned output_length = _output_buffer.get_length();
+				for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
-				for (unsigned i=0; i < output_length; i++) {
+					_output_buffer[i].quat_nominal *= _state_reset_status.quat_change;
-					output_states = _output_buffer.get_from_index(i);
-					output_states.quat_nominal = _state_reset_status.quat_change * output_states.quat_nominal;
-					_output_buffer.push_to_index(i, output_states);
 				}
 
 				// apply the change in attitude quaternion to our newest quaternion estimate
@@ -530,8 +529,6 @@ void Ekf::controlGpsFusion()
 
 			// set innovation gate size
 			_posInnovGateNE = fmaxf(_params.posNE_innov_gate, 1.0f);
-
-
 		}
 
 	} else if (_control_status.flags.gps && (_time_last_imu - _time_last_gps > (uint64_t)10e6)) {
@@ -588,10 +585,11 @@ void Ekf::controlHeightSensorTimeouts()
 		// handle the case where we are using baro for height
 		if (_control_status.flags.baro_hgt) {
 			// check if GPS height is available
-			gpsSample gps_init = _gps_buffer.get_newest();
+			const gpsSample& gps_init = _gps_buffer.get_newest();
 			bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
 			bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
-			baroSample baro_init = _baro_buffer.get_newest();
+
+			const baroSample& baro_init = _baro_buffer.get_newest();
 			bool baro_hgt_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
 
 			// check for inertial sensing errors in the last 10 seconds
@@ -642,12 +640,12 @@ void Ekf::controlHeightSensorTimeouts()
 		// handle the case we are using GPS for height
 		if (_control_status.flags.gps_hgt) {
 			// check if GPS height is available
-			gpsSample gps_init = _gps_buffer.get_newest();
+			const gpsSample& gps_init = _gps_buffer.get_newest();
 			bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
 			bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
 
 			// check the baro height source for consistency and freshness
-			baroSample baro_init = _baro_buffer.get_newest();
+			const baroSample& baro_init = _baro_buffer.get_newest();
 			bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
 			float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
 			bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P[8][8]) * sq(_params.baro_innov_gate);
@@ -694,11 +692,11 @@ void Ekf::controlHeightSensorTimeouts()
 		// handle the case we are using range finder for height
 		if (_control_status.flags.rng_hgt) {
 			// check if range finder data is available
-			rangeSample rng_init = _range_buffer.get_newest();
+			const rangeSample& rng_init = _range_buffer.get_newest();
 			bool rng_data_available = ((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL);
 
 			// check if baro data is available
-			baroSample baro_init = _baro_buffer.get_newest();
+			const baroSample& baro_init = _baro_buffer.get_newest();
 			bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
 
 			// reset to baro if we have no range data and baro data is available
@@ -740,11 +738,11 @@ void Ekf::controlHeightSensorTimeouts()
 		// handle the case where we are using external vision data for height
 		if (_control_status.flags.ev_hgt) {
 			// check if vision data is available
-			extVisionSample ev_init = _ext_vision_buffer.get_newest();
+			const extVisionSample& ev_init = _ext_vision_buffer.get_newest();
 			bool ev_data_available = ((_time_last_imu - ev_init.time_us) < 2 * EV_MAX_INTERVAL);
 
 			// check if baro data is available
-			baroSample baro_init = _baro_buffer.get_newest();
+			const baroSample& baro_init = _baro_buffer.get_newest();
 			bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
 
 			// reset to baro if we have no vision data and baro data is available

EKF/ekf.cpp

@@ -138,7 +138,7 @@ bool Ekf::initialiseFilter()
 	// Keep accumulating measurements until we have a minimum of 10 samples for the required sensors
 
 	// Sum the IMU delta angle measurements
-	imuSample imu_init = _imu_buffer.get_newest();
+	const imuSample& imu_init = _imu_buffer.get_newest();
 	_delVel_sum += imu_init.delta_vel;
 
 	// Sum the magnetometer measurements
@@ -281,7 +281,7 @@ bool Ekf::initialiseFilter()
 		if (_control_status.flags.rng_hgt) {
 			// if we are using the range finder as the primary source, then calculate the baro height at origin so  we can use baro as a backup
 			// so it can be used as a backup ad set the initial height using the range finder
-			baroSample baro_newest = _baro_buffer.get_newest();
+			const baroSample& baro_newest = _baro_buffer.get_newest();
 			_baro_hgt_offset = baro_newest.hgt;
 			_state.pos(2) = -math::max(_rng_filt_state * _R_rng_to_earth_2_2, _params.rng_gnd_clearance);
 			ECL_INFO("EKF using range finder height - commencing alignment");
@@ -379,11 +379,7 @@ bool Ekf::collect_imu(imuSample &imu)
 	// accumulate and downsample IMU data across a period FILTER_UPDATE_PERIOD_MS long
 
 	// copy imu data to local variables
-	_imu_sample_new.delta_ang	= imu.delta_ang;
+	_imu_sample_new = imu;
-	_imu_sample_new.delta_vel	= imu.delta_vel;
-	_imu_sample_new.delta_ang_dt	= imu.delta_ang_dt;
-	_imu_sample_new.delta_vel_dt	= imu.delta_vel_dt;
-	_imu_sample_new.time_us		= imu.time_us;
 
 	// accumulate the time deltas
 	_imu_down_sampled.delta_ang_dt += imu.delta_ang_dt;
@@ -393,7 +389,7 @@ bool Ekf::collect_imu(imuSample &imu)
 	// this quaternion represents the rotation from the start to end of the accumulation period
 	Quatf delta_q;
 	delta_q.rotate(imu.delta_ang);
-	_q_down_sampled =  _q_down_sampled * delta_q;
+	_q_down_sampled = _q_down_sampled * delta_q;
 	_q_down_sampled.normalize();
 
 	// rotate the accumulated delta velocity data forward each time so it is always in the updated rotation frame
@@ -406,7 +402,7 @@ bool Ekf::collect_imu(imuSample &imu)
 
 	// if the target time delta between filter prediction steps has been exceeded
 	// write the accumulated IMU data to the ring buffer
-	float target_dt = (float)(FILTER_UPDATE_PERIOD_MS) / 1000;
+	const float target_dt = FILTER_UPDATE_PERIOD_MS / 1000.0f;
 
 	if (_imu_down_sampled.delta_ang_dt >= target_dt - _imu_collection_time_adj) {
 
@@ -446,7 +442,7 @@ bool Ekf::collect_imu(imuSample &imu)
 void Ekf::calculateOutputStates()
 {
 	// Use full rate IMU data at the current time horizon
-	imuSample imu_new = _imu_sample_new;
+	const imuSample& imu_new = _imu_sample_new;
 
 	// correct delta angles for bias offsets
 	Vector3f delta_angle;
@@ -496,7 +492,7 @@ void Ekf::calculateOutputStates()
 		_vel_deriv_ned = delta_vel_NED * (1.0f / imu_new.delta_vel_dt);
 	}
 
-	// save the previous velocity so we can use trapezidal integration
+	// save the previous velocity so we can use trapezoidal integration
 	Vector3f vel_last = _output_new.vel;
 
 	// increment the INS velocity states by the measurement plus corrections
@@ -539,7 +535,7 @@ void Ekf::calculateOutputStates()
 
 		// calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon
 		Quatf quat_inv = _state.quat_nominal.inversed();
-		Quatf q_error =   quat_inv * _output_sample_delayed.quat_nominal;
+		Quatf q_error =  quat_inv * _output_sample_delayed.quat_nominal;
 		q_error.normalize();
 
 		// convert the quaternion delta to a delta angle
@@ -579,7 +575,7 @@ void Ekf::calculateOutputStates()
 		/*
 		 * Loop through the output filter state history and apply the corrections to the velocity and position states.
 		 * This method is too expensive to use for the attitude states due to the quaternion operations required
-		 * but becasue it eliminates the time delay in the 'correction loop' it allows higher tracking gains
+		 * but because it eliminates the time delay in the 'correction loop' it allows higher tracking gains
 		 * to be used and reduces tracking error relative to EKF states.
 		 */
 
@@ -612,30 +608,24 @@ void Ekf::calculateOutputStates()
 
 			// loop through the vertical output filter state history starting at the oldest and apply the corrections to the
 			// vel_d states and propagate vel_d_integ forward using the corrected vel_d
-			outputVert current_state;
+			uint8_t index = _output_vert_buffer.get_oldest_index();
-			outputVert next_state;
-			unsigned index = _output_vert_buffer.get_oldest_index();
-			unsigned index_next;
-			unsigned size = _output_vert_buffer.get_length();
 
-			for (unsigned counter = 0; counter < (size - 1); counter++) {
+			const uint8_t size = _output_vert_buffer.get_length();
-				index_next = (index + 1) % size;
+
-				current_state = _output_vert_buffer.get_from_index(index);
+			for (uint8_t counter = 0; counter < (size - 1); counter++) {
-				next_state = _output_vert_buffer.get_from_index(index_next);
+				const uint8_t index_next = (index + 1) % size;
+				outputVert& current_state = _output_vert_buffer[index];
+				outputVert& next_state = _output_vert_buffer[index_next];
 
 				// correct the velocity
 				if (counter == 0) {
 					current_state.vel_d += vel_d_correction;
-					_output_vert_buffer.push_to_index(index, current_state);
 				}
 				next_state.vel_d += vel_d_correction;
 
 				// position is propagated forward using the corrected velocity and a trapezoidal integrator
 				next_state.vel_d_integ = current_state.vel_d_integ + (current_state.vel_d + next_state.vel_d) * 0.5f * next_state.dt;
 
-				// push the updated data to the buffer
-				_output_vert_buffer.push_to_index(index_next, next_state);
-
 				// advance the index
 				index = (index + 1) % size;
 			}
@@ -663,20 +653,12 @@ void Ekf::calculateOutputStates()
 			Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f;
 
 			// loop through the output filter state history and apply the corrections to the velocity and position states
-			outputSample output_states;
+			for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
-			unsigned max_index = _output_buffer.get_length() - 1;
+				// a constant velocity correction is applied
-			for (unsigned index = 0; index <= max_index; index++) {
+				_output_buffer[index].vel += vel_correction;
-				output_states = _output_buffer.get_from_index(index);
-
-				// a constant  velocity correction is applied
-				output_states.vel += vel_correction;
 
 				// a constant position correction is applied
-				output_states.pos += pos_correction;
+				_output_buffer[index].pos += pos_correction;
-
-				// push the updated data to the buffer
-				_output_buffer.push_to_index(index, output_states);
-
 			}
 
 			// update output state to corrected values

EKF/ekf_helper.cpp

@@ -68,15 +68,10 @@ bool Ekf::resetVelocity()
 	}
 
 	// calculate the change in velocity and apply to the output predictor state history
-	Vector3f velocity_change = _state.vel - vel_before_reset;
+	const Vector3f velocity_change = _state.vel - vel_before_reset;
-	outputSample output_states = {};
-	unsigned max_index = _output_buffer.get_length() - 1;
-
-	for (unsigned index = 0; index <= max_index; index++) {
-		output_states = _output_buffer.get_from_index(index);
-		output_states.vel += velocity_change;
-		_output_buffer.push_to_index(index, output_states);
 
+	for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
+		_output_buffer[index].vel += velocity_change;
 	}
 
 	// apply the change in velocity to our newest velocity estimate
@@ -131,17 +126,11 @@ bool Ekf::resetPosition()
 	}
 
 	// calculate the change in position and apply to the output predictor state history
-	Vector2f posNE_change;
+	const Vector2f posNE_change{_state.pos(0) - posNE_before_reset(0), _state.pos(1) - posNE_before_reset(1)};
-	posNE_change(0) = _state.pos(0) - posNE_before_reset(0);
-	posNE_change(1) = _state.pos(1) - posNE_before_reset(1);
-	outputSample output_states = {};
-	unsigned max_index = _output_buffer.get_length() - 1;
 
-	for (unsigned index = 0; index <= max_index; index++) {
+	for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
-		output_states = _output_buffer.get_from_index(index);
+		_output_buffer[index].pos(0) += posNE_change(0);
-		output_states.pos(0) += posNE_change(0);
+		_output_buffer[index].pos(1) += posNE_change(1);
-		output_states.pos(1) += posNE_change(1);
-		_output_buffer.push_to_index(index, output_states);
 	}
 
 	// apply the change in position to our newest position estimate
@@ -160,7 +149,7 @@ bool Ekf::resetPosition()
 void Ekf::resetHeight()
 {
 	// Get the most recent GPS data
-	gpsSample gps_newest = _gps_buffer.get_newest();
+	const gpsSample& gps_newest = _gps_buffer.get_newest();
 
 	// store the current vertical position and velocity for reference so we can calculate and publish the reset amount
 	float old_vert_pos = _state.pos(2);
@@ -193,7 +182,7 @@ void Ekf::resetHeight()
 			vert_pos_reset = true;
 
 			// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
-			baroSample baro_newest = _baro_buffer.get_newest();
+			const baroSample& baro_newest = _baro_buffer.get_newest();
 			_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
 
 		} else {
@@ -203,7 +192,7 @@ void Ekf::resetHeight()
 
 	} else if (_control_status.flags.baro_hgt) {
 		// initialize vertical position with newest baro measurement
-		baroSample baro_newest = _baro_buffer.get_newest();
+		const baroSample& baro_newest = _baro_buffer.get_newest();
 
 		if (_time_last_imu - baro_newest.time_us < 2 * BARO_MAX_INTERVAL) {
 			_state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset;
@@ -236,7 +225,7 @@ void Ekf::resetHeight()
 			vert_pos_reset = true;
 
 			// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
-			baroSample baro_newest = _baro_buffer.get_newest();
+			const baroSample& baro_newest = _baro_buffer.get_newest();
 			_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
 
 		} else {
@@ -245,7 +234,7 @@ void Ekf::resetHeight()
 
 	} else if (_control_status.flags.ev_hgt) {
 		// initialize vertical position with newest measurement
-		extVisionSample ev_newest = _ext_vision_buffer.get_newest();
+		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
 		int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us;
@@ -306,22 +295,14 @@ void Ekf::resetHeight()
 	}
 
 	// add the reset amount to the output observer buffered data
-	outputSample output_states = {};
+	for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
-	unsigned output_length = _output_buffer.get_length();
-
-	for (unsigned i = 0; i < output_length; i++) {
-		output_states = _output_buffer.get_from_index(i);
-
 		if (vert_pos_reset) {
-			output_states.pos(2) += _state_reset_status.posD_change;
+			_output_buffer[i].pos(2) += _state_reset_status.posD_change;
 		}
 
 		if (vert_vel_reset) {
-			output_states.vel(2) += _state_reset_status.velD_change;
+			_output_buffer[i].vel(2) += _state_reset_status.velD_change;
 		}
-
-		_output_buffer.push_to_index(i, output_states);
-
 	}
 }
 
@@ -329,25 +310,20 @@ void Ekf::resetHeight()
 void Ekf::alignOutputFilter()
 {
 	// calculate the quaternion delta between the output and EKF quaternions at the EKF fusion time horizon
-	Quatf quat_inv = _state.quat_nominal.inversed();
+	Quatf q_delta = _state.quat_nominal.inversed() * _output_sample_delayed.quat_nominal;
-	Quatf q_delta =   quat_inv * _output_sample_delayed.quat_nominal;
 	q_delta.normalize();
 
 	// calculate the velocity and posiiton deltas between the output and EKF at the EKF fusion time horizon
-	Vector3f vel_delta = _state.vel - _output_sample_delayed.vel;
+	const Vector3f vel_delta = _state.vel - _output_sample_delayed.vel;
-	Vector3f pos_delta = _state.pos - _output_sample_delayed.pos;
+	const Vector3f pos_delta = _state.pos - _output_sample_delayed.pos;
 
 	// loop through the output filter state history and add the deltas
-	outputSample output_states = {};
+	// Note q1 *= q2 is equivalent to q1 = q2 * q1
-	unsigned output_length = _output_buffer.get_length();
+	for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
-
+		_output_buffer[i].quat_nominal *= q_delta;
-	for (unsigned i = 0; i < output_length; i++) {
+		_output_buffer[i].quat_nominal.normalize();
-		output_states = _output_buffer.get_from_index(i);
+		_output_buffer[i].vel += vel_delta;
-		output_states.quat_nominal = q_delta * output_states.quat_nominal;
+		_output_buffer[i].pos += pos_delta;
-		output_states.quat_nominal.normalize();
-		output_states.vel += vel_delta;
-		output_states.pos += pos_delta;
-		_output_buffer.push_to_index(i, output_states);
 	}
 }
 
@@ -454,12 +430,9 @@ bool Ekf::realignYawGPS()
 			_state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal;
 
 			// add the reset amount to the output observer buffered data
-			outputSample output_states;
+			// Note q1 *= q2 is equivalent to q1 = q2 * q1
-			unsigned output_length = _output_buffer.get_length();
+			for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
-			for (unsigned i = 0; i < output_length; i++) {
+				_output_buffer[i].quat_nominal *= _state_reset_status.quat_change;
-				output_states = _output_buffer.get_from_index(i);
-				output_states.quat_nominal = _state_reset_status.quat_change * output_states.quat_nominal;
-				_output_buffer.push_to_index(i, output_states);
 			}
 
 			// apply the change in attitude quaternion to our newest quaternion estimate
@@ -679,13 +652,9 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 		initialiseQuatCovariances(angle_err_var_vec);
 
 		// add the reset amount to the output observer buffered data
-		outputSample output_states = {};
+		for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
-		unsigned output_length = _output_buffer.get_length();
+			// Note q1 *= q2 is equivalent to q1 = q2 * q1
-
+			_output_buffer[i].quat_nominal *= _state_reset_status.quat_change;
-		for (unsigned i = 0; i < output_length; i++) {
-			output_states = _output_buffer.get_from_index(i);
-			output_states.quat_nominal = _state_reset_status.quat_change * output_states.quat_nominal;
-			_output_buffer.push_to_index(i, output_states);
 		}
 
 		// apply the change in attitude quaternion to our newest quaternion estimate

EKF/estimator_interface.cpp

@@ -55,9 +55,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
 		_initialised = true;
 	}
 
-	float dt = (float)(time_usec - _time_last_imu) / 1000 / 1000;
+	const float dt = math::constrain((time_usec - _time_last_imu) / 1e6f, 1.0e-4f, 0.02f);
-	dt = math::max(dt, 1.0e-4f);
-	dt = math::min(dt, 0.02f);
 
 	_time_last_imu = time_usec;
 
@@ -66,7 +64,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
 	}
 
 	// copy data
-	imuSample imu_sample_new = {};
+	imuSample imu_sample_new;
 	imu_sample_new.delta_ang = Vector3f(delta_ang);
 	imu_sample_new.delta_vel = Vector3f(delta_vel);
 
@@ -151,7 +149,7 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3])
 	// limit data rate to prevent data being lost
 	if (time_usec - _time_last_mag > _min_obs_interval_us) {
 
-		magSample mag_sample_new = {};
+		magSample mag_sample_new;
 		mag_sample_new.time_us = time_usec - _params.mag_delay_ms * 1000;
 
 		mag_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
@@ -173,7 +171,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
 	bool need_gps = (_params.fusion_mode & MASK_USE_GPS) || (_params.vdist_sensor_type == VDIST_SENSOR_GPS);
 
 	if (((time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps && gps->fix_type > 2) {
-		gpsSample gps_sample_new = {};
+		gpsSample gps_sample_new;
 		gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000;
 
 		gps_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
@@ -215,7 +213,7 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float data)
 	// limit data rate to prevent data being lost
 	if (time_usec - _time_last_baro > _min_obs_interval_us) {
 
-		baroSample baro_sample_new{};
+		baroSample baro_sample_new;
 		baro_sample_new.hgt = data;
 		baro_sample_new.time_us = time_usec - _params.baro_delay_ms * 1000;
 
@@ -236,7 +234,7 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed
 
 	// limit data rate to prevent data being lost
 	if (time_usec - _time_last_airspeed > _min_obs_interval_us) {
-		airspeedSample airspeed_sample_new{};
+		airspeedSample airspeed_sample_new;
 		airspeed_sample_new.true_airspeed = true_airspeed;
 		airspeed_sample_new.eas2tas = eas2tas;
 		airspeed_sample_new.time_us = time_usec - _params.airspeed_delay_ms * 1000;
@@ -246,8 +244,7 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed
 		_airspeed_buffer.push(airspeed_sample_new);
 	}
 }
-static float rng;
+
-// set range data
 void EstimatorInterface::setRangeData(uint64_t time_usec, float data)
 {
 	if (!_initialised) {
@@ -256,9 +253,8 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float data)
 
 	// limit data rate to prevent data being lost
 	if (time_usec - _time_last_range > _min_obs_interval_us) {
-		rangeSample range_sample_new = {};
+		rangeSample range_sample_new;
 		range_sample_new.rng = data;
-		rng = data;
 		range_sample_new.time_us = time_usec - _params.range_delay_ms * 1000;
 		_time_last_range = time_usec;
 

EKF/terrain_estimator.cpp

@@ -45,7 +45,7 @@
 bool Ekf::initHagl()
 {
 	// get most recent range measurement from buffer
-	rangeSample latest_measurement = _range_buffer.get_newest();
+	const rangeSample& latest_measurement = _range_buffer.get_newest();
 
 	if ((_time_last_imu - latest_measurement.time_us) < (uint64_t)2e5 && _R_rng_to_earth_2_2 > _params.range_cos_max_tilt) {
 		// if we have a fresh measurement, use it to initialise the terrain estimator