From e397cbf8bbf065c0fef88e5fdaf31f7f64fe3009 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 10 Nov 2020 10:02:49 -0500
Subject: [PATCH] ekf2: move ekf_gps_drift publication to method

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

diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index 92a9a712c3..2f51e17bee 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -923,21 +923,6 @@ void EKF2::Run()
 				_estimator_sensor_bias_pub.publish(bias);
 			}
 
-			// publish GPS drift data only when updated to minimise overhead
-			float gps_drift[3];
-			bool blocked;
-
-			if (_ekf.get_gps_drift_metrics(gps_drift, &blocked)) {
-				ekf_gps_drift_s drift_data;
-				drift_data.hpos_drift_rate = gps_drift[0];
-				drift_data.vpos_drift_rate = gps_drift[1];
-				drift_data.hspd = gps_drift[2];
-				drift_data.blocked = blocked;
-				drift_data.timestamp = _replay_mode ? now : hrt_absolute_time();
-
-				_ekf_gps_drift_pub.publish(drift_data);
-			}
-
 			{
 				/* Check and save learned magnetometer bias estimates */
 
@@ -1010,6 +995,9 @@ void EKF2::Run()
 			PublishWindEstimate(now);
 			PublishYawEstimatorStatus(now);
 
+			// publish status/logging messages
+			PublishEkfDriftMetrics(now);
+
 			if (!_mag_decl_saved && _standby) {
 				_mag_decl_saved = update_mag_decl(_param_ekf2_mag_decl);
 			}
@@ -1239,6 +1227,24 @@ void EKF2::PublishAttitude(const hrt_abstime &timestamp)
 	}
 }
 
+void EKF2::PublishEkfDriftMetrics(const hrt_abstime &timestamp)
+{
+	// publish GPS drift data only when updated to minimise overhead
+	float gps_drift[3];
+	bool blocked;
+
+	if (_ekf.get_gps_drift_metrics(gps_drift, &blocked)) {
+		ekf_gps_drift_s drift_data;
+		drift_data.hpos_drift_rate = gps_drift[0];
+		drift_data.vpos_drift_rate = gps_drift[1];
+		drift_data.hspd = gps_drift[2];
+		drift_data.blocked = blocked;
+		drift_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+
+		_ekf_gps_drift_pub.publish(drift_data);
+	}
+}
+
 void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu)
 {
 	// generate vehicle odometry data
diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp
index 4a0abb813e..c1b7037387 100644
--- a/src/modules/ekf2/EKF2.hpp
+++ b/src/modules/ekf2/EKF2.hpp
@@ -135,6 +135,7 @@ private:
 	bool update_mag_decl(Param &mag_decl_param);
 
 	void PublishAttitude(const hrt_abstime &timestamp);
+	void PublishEkfDriftMetrics(const hrt_abstime &timestamp);
 	void PublishOpticalFlowVel(const hrt_abstime &timestamp, const optical_flow_s &optical_flow);
 	void PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu);
 	void PublishWindEstimate(const hrt_abstime &timestamp);