From 651e292d42b64e8d6ab1abd62288b62735b403ea Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sat, 31 Oct 2020 15:14:51 -0400
Subject: [PATCH] ekf2: move AirspeedSample update to UpdateAirspeedSample()

---
 src/modules/ekf2/EKF2.cpp | 41 ++++++++++++++++++++++-----------------
 src/modules/ekf2/EKF2.hpp |  2 ++
 2 files changed, 25 insertions(+), 18 deletions(-)

diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index 76c6d6cb0c..438d11917c 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -451,24 +451,7 @@ void EKF2::Run()
 			}
 		}
 
-		if (_airspeed_sub.updated()) {
-			airspeed_s airspeed;
-
-			if (_airspeed_sub.copy(&airspeed)) {
-				// only set airspeed data if condition for airspeed fusion are met
-				if ((_param_ekf2_arsp_thr.get() > FLT_EPSILON) && (airspeed.true_airspeed_m_s > _param_ekf2_arsp_thr.get())) {
-
-					airspeedSample airspeed_sample {};
-					airspeed_sample.time_us = airspeed.timestamp;
-					airspeed_sample.eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;
-					airspeed_sample.true_airspeed = airspeed.true_airspeed_m_s;
-					_ekf.setAirspeedData(airspeed_sample);
-				}
-
-				ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 -
-						(int64_t)ekf2_timestamps.timestamp / 100);
-			}
-		}
+		UpdateAirspeedSample(ekf2_timestamps);
 
 		bool new_optical_flow_data_received = false;
 		optical_flow_s optical_flow;
@@ -1362,6 +1345,28 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
 	return amsl_hgt + _wgs84_hgt_offset;
 }
 
+void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
+{
+	// EKF airspeed sample
+	airspeed_s airspeed;
+
+	if (_airspeed_sub.update(&airspeed)) {
+		// only set airspeed data if condition for airspeed fusion are met
+		if ((_param_ekf2_arsp_thr.get() > FLT_EPSILON) && (airspeed.true_airspeed_m_s > _param_ekf2_arsp_thr.get())) {
+
+			airspeedSample airspeed_sample {
+				.true_airspeed = airspeed.true_airspeed_m_s,
+				.eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s,
+				.time_us = airspeed.timestamp,
+			};
+			_ekf.setAirspeedData(airspeed_sample);
+		}
+
+		ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 -
+				(int64_t)ekf2_timestamps.timestamp / 100);
+	}
+}
+
 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 d6bc21bd86..7630ee7f89 100644
--- a/src/modules/ekf2/EKF2.hpp
+++ b/src/modules/ekf2/EKF2.hpp
@@ -146,6 +146,8 @@ private:
 	void PublishWindEstimate(const hrt_abstime &timestamp);
 	void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
 
+	void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps);
+
 	void UpdateMagCalibration(const hrt_abstime &timestamp);
 
 	/*