From 68f49619b71c6df5817d7a99c397d7be9b14b480 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sat, 31 Oct 2020 15:29:37 -0400
Subject: [PATCH] ekf2: move gps_message update to UpdateGpsSample()

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

diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index 54afb9ad78..39600230e9 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -423,20 +423,7 @@ void EKF2::Run()
 
 		UpdateBaroSample(ekf2_timestamps);
 
-		if (_vehicle_gps_position_sub.updated()) {
-			vehicle_gps_position_s gps;
-
-			if (_vehicle_gps_position_sub.copy(&gps)) {
-				gps_message gps_msg{};
-
-				fillGpsMsgWithVehicleGpsPosData(gps_msg, gps);
-
-				_ekf.setGpsData(gps_msg);
-
-				_gps_time_usec = gps_msg.time_usec;
-				_gps_alttitude_ellipsoid = gps.alt_ellipsoid;
-			}
-		}
+		UpdateGpsSample(ekf2_timestamps);
 
 		UpdateAirspeedSample(ekf2_timestamps);
 
@@ -538,27 +525,6 @@ void EKF2::Run()
 	}
 }
 
-void EKF2::fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_position_s &data)
-{
-	msg.time_usec = data.timestamp;
-	msg.lat = data.lat;
-	msg.lon = data.lon;
-	msg.alt = data.alt;
-	msg.yaw = data.heading;
-	msg.yaw_offset = data.heading_offset;
-	msg.fix_type = data.fix_type;
-	msg.eph = data.eph;
-	msg.epv = data.epv;
-	msg.sacc = data.s_variance_m_s;
-	msg.vel_m_s = data.vel_m_s;
-	msg.vel_ned(0) = data.vel_n_m_s;
-	msg.vel_ned(1) = data.vel_e_m_s;
-	msg.vel_ned(2) = data.vel_d_m_s;
-	msg.vel_ned_valid = data.vel_ned_valid;
-	msg.nsats = data.satellites_used;
-	msg.pdop = sqrtf(data.hdop * data.hdop + data.vdop * data.vdop);
-}
-
 void EKF2::runPreFlightChecks(const float dt,
 			      const filter_control_status_u &control_status,
 			      const estimator_innovations_s &innov,
@@ -1401,6 +1367,43 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps, optical_flow_s &
 	return new_optical_flow;
 }
 
+void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
+{
+	// EKF GPS message
+	if (_param_ekf2_aid_mask.get() & MASK_USE_GPS) {
+		vehicle_gps_position_s vehicle_gps_position;
+
+		if (_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
+			gps_message gps_msg{
+				.time_usec = vehicle_gps_position.timestamp,
+				.lat = vehicle_gps_position.lat,
+				.lon = vehicle_gps_position.lon,
+				.alt = vehicle_gps_position.alt,
+				.yaw = vehicle_gps_position.heading,
+				.yaw_offset = vehicle_gps_position.heading_offset,
+				.fix_type = vehicle_gps_position.fix_type,
+				.eph = vehicle_gps_position.eph,
+				.epv = vehicle_gps_position.epv,
+				.sacc = vehicle_gps_position.s_variance_m_s,
+				.vel_m_s = vehicle_gps_position.vel_m_s,
+				.vel_ned = Vector3f{
+					vehicle_gps_position.vel_n_m_s,
+					vehicle_gps_position.vel_e_m_s,
+					vehicle_gps_position.vel_d_m_s
+				},
+				.vel_ned_valid = vehicle_gps_position.vel_ned_valid,
+				.nsats = vehicle_gps_position.satellites_used,
+				.pdop = sqrtf(vehicle_gps_position.hdop *vehicle_gps_position.hdop
+					      + vehicle_gps_position.vdop * vehicle_gps_position.vdop),
+			};
+			_ekf.setGpsData(gps_msg);
+
+			_gps_time_usec = gps_msg.time_usec;
+			_gps_alttitude_ellipsoid = vehicle_gps_position.alt_ellipsoid;
+		}
+	}
+}
+
 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 bb59d70e20..11ebb9b192 100644
--- a/src/modules/ekf2/EKF2.hpp
+++ b/src/modules/ekf2/EKF2.hpp
@@ -121,7 +121,6 @@ private:
 	void Run() override;
 
 	int getRangeSubIndex(); ///< get subscription index of first downward-facing range sensor
-	void fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_position_s &data);
 
 	PreFlightChecker _preflt_checker;
 	void runPreFlightChecks(float dt, const filter_control_status_u &control_status,
@@ -152,6 +151,7 @@ private:
 	void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps);
 	bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom);
 	bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps, optical_flow_s &optical_flow);
+	void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
 
 	void UpdateMagCalibration(const hrt_abstime &timestamp);