From 8a0581516c2f6279ed7b6bf83dddf376e368b12e Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 1 Feb 2022 09:48:03 -0500
Subject: [PATCH] ekf2: don't store _auxvel_sample_delayed

---
 src/modules/ekf2/EKF/control.cpp           | 28 +++++++++++-----------
 src/modules/ekf2/EKF/estimator_interface.h |  1 -
 2 files changed, 14 insertions(+), 15 deletions(-)

diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp
index b8d6a6279a..5b6c96726b 100644
--- a/src/modules/ekf2/EKF/control.cpp
+++ b/src/modules/ekf2/EKF/control.cpp
@@ -1068,27 +1068,27 @@ void Ekf::controlDragFusion()
 
 void Ekf::controlAuxVelFusion()
 {
-	bool data_ready = false;
-
 	if (_auxvel_buffer) {
-		data_ready = _auxvel_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_auxvel_sample_delayed);
-	}
+		auxVelSample auxvel_sample_delayed;
 
-	if (data_ready && isHorizontalAidingActive()) {
+		if (_auxvel_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &auxvel_sample_delayed)) {
 
-		const Vector2f aux_vel_innov_gate(_params.auxvel_gate, _params.auxvel_gate);
+			if (isHorizontalAidingActive()) {
 
-		_last_vel_obs = _auxvel_sample_delayed.vel;
-		_aux_vel_innov = _state.vel - _last_vel_obs;
-		_last_vel_obs_var = _aux_vel_innov_var;
+				const Vector2f aux_vel_innov_gate(_params.auxvel_gate, _params.auxvel_gate);
 
-		fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, _auxvel_sample_delayed.velVar,
-				       _aux_vel_innov_var, _aux_vel_test_ratio);
+				_last_vel_obs = auxvel_sample_delayed.vel;
+				_aux_vel_innov = _state.vel - _last_vel_obs;
+				_last_vel_obs_var = _aux_vel_innov_var;
 
-		// Can be enabled after bit for this is added to EKF_AID_MASK
-		// fuseVerticalVelocity(_aux_vel_innov, aux_vel_innov_gate, _auxvel_sample_delayed.velVar,
-		//		_aux_vel_innov_var, _aux_vel_test_ratio);
+				fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, auxvel_sample_delayed.velVar,
+						       _aux_vel_innov_var, _aux_vel_test_ratio);
 
+				// Can be enabled after bit for this is added to EKF_AID_MASK
+				// fuseVerticalVelocity(_aux_vel_innov, aux_vel_innov_gate, auxvel_sample_delayed.velVar,
+				//		_aux_vel_innov_var, _aux_vel_test_ratio);
+			}
+		}
 	}
 }
 
diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h
index c1cfb58118..b394784d4f 100644
--- a/src/modules/ekf2/EKF/estimator_interface.h
+++ b/src/modules/ekf2/EKF/estimator_interface.h
@@ -296,7 +296,6 @@ protected:
 	extVisionSample _ev_sample_delayed{};
 	extVisionSample _ev_sample_delayed_prev{};
 	dragSample _drag_down_sampled{};	// down sampled drag specific force data (filter prediction rate -> observation rate)
-	auxVelSample _auxvel_sample_delayed{};
 
 	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};		// air density (kg/m**3)