From 6b30f01daf6d879e58d73c30abe51e5f44d53534 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sat, 31 Oct 2020 15:16:14 -0400
Subject: [PATCH] ekf2: move AuxVelSample update to UpdateAuxVelSample()

---
 src/modules/ekf2/EKF2.cpp | 37 +++++++++++++++++++++----------------
 src/modules/ekf2/EKF2.hpp |  1 +
 2 files changed, 22 insertions(+), 16 deletions(-)

diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index 438d11917c..aa8aab94dd 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -606,22 +606,7 @@ void EKF2::Run()
 			}
 		}
 
-		// use the landing target pose estimate as another source of velocity data
-		if (_landing_target_pose_sub.updated()) {
-			landing_target_pose_s landing_target_pose;
-
-			if (_landing_target_pose_sub.copy(&landing_target_pose)) {
-				// we can only use the landing target if it has a fixed position and  a valid velocity estimate
-				if (landing_target_pose.is_static && landing_target_pose.rel_vel_valid) {
-					// velocity of vehicle relative to target has opposite sign to target relative to vehicle
-					auxVelSample auxvel_sample {};
-					auxvel_sample.vel = Vector3f{-landing_target_pose.vx_rel, -landing_target_pose.vy_rel, 0.0f};
-					auxvel_sample.velVar = Vector3f{landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel, 0.0f};
-					auxvel_sample.time_us = landing_target_pose.timestamp;
-					_ekf.setAuxVelData(auxvel_sample);
-				}
-			}
-		}
+		UpdateAuxVelSample(ekf2_timestamps);
 
 		// run the EKF update and output
 		perf_begin(_ekf_update_perf);
@@ -1367,6 +1352,26 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
 	}
 }
 
+void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
+{
+	// EKF auxillary velocity sample
+	//  - use the landing target pose estimate as another source of velocity data
+	landing_target_pose_s landing_target_pose;
+
+	if (_landing_target_pose_sub.update(&landing_target_pose)) {
+		// we can only use the landing target if it has a fixed position and  a valid velocity estimate
+		if (landing_target_pose.is_static && landing_target_pose.rel_vel_valid) {
+			// velocity of vehicle relative to target has opposite sign to target relative to vehicle
+			auxVelSample auxvel_sample{
+				.vel = Vector3f{-landing_target_pose.vx_rel, -landing_target_pose.vy_rel, 0.0f},
+				.velVar = Vector3f{landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel, 0.0f},
+				.time_us = landing_target_pose.timestamp,
+			};
+			_ekf.setAuxVelData(auxvel_sample);
+		}
+	}
+}
+
 void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
 {
 	/* Check and save learned magnetometer bias estimates */
diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp
index 7630ee7f89..aa51caff39 100644
--- a/src/modules/ekf2/EKF2.hpp
+++ b/src/modules/ekf2/EKF2.hpp
@@ -147,6 +147,7 @@ private:
 	void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
 
 	void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps);
+	void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps);
 
 	void UpdateMagCalibration(const hrt_abstime &timestamp);