From 0f567931168e22b3b7b420913c3d169b6f5cd437 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sat, 31 Oct 2020 15:18:29 -0400
Subject: [PATCH] ekf2: move BaroSample update to UpdateBaroSample()

---
 src/modules/ekf2/EKF2.cpp | 33 +++++++++++++++++++--------------
 src/modules/ekf2/EKF2.hpp |  1 +
 2 files changed, 20 insertions(+), 14 deletions(-)

diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index aa8aab94dd..8c859c4745 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -421,20 +421,7 @@ void EKF2::Run()
 			}
 		}
 
-		// read baro data
-		if (_airdata_sub.updated()) {
-			vehicle_air_data_s airdata;
-
-			if (_airdata_sub.copy(&airdata)) {
-				_ekf.set_air_density(airdata.rho);
-				const baroSample baro_sample {airdata.baro_alt_meter, airdata.timestamp_sample};
-				_ekf.setBaroData(baro_sample);
-				ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 -
-						(int64_t)ekf2_timestamps.timestamp / 100);
-
-				_device_id_baro = airdata.baro_device_id;
-			}
-		}
+		UpdateBaroSample(ekf2_timestamps);
 
 		if (_vehicle_gps_position_sub.updated()) {
 			vehicle_gps_position_s gps;
@@ -1372,6 +1359,24 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
 	}
 }
 
+void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
+{
+	// EKF baro sample
+	vehicle_air_data_s airdata;
+
+	if (_airdata_sub.update(&airdata)) {
+		_ekf.set_air_density(airdata.rho);
+
+		const baroSample baro_sample {airdata.baro_alt_meter, airdata.timestamp_sample};
+		_ekf.setBaroData(baro_sample);
+
+		_device_id_baro = airdata.baro_device_id;
+
+		ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.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 aa51caff39..7882000e9d 100644
--- a/src/modules/ekf2/EKF2.hpp
+++ b/src/modules/ekf2/EKF2.hpp
@@ -148,6 +148,7 @@ private:
 
 	void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps);
+	void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps);
 
 	void UpdateMagCalibration(const hrt_abstime &timestamp);