From 56f0ef9e4163f7b37ccc84ab34a131046ac4e31f Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 15 Feb 2024 10:12:50 -0500
Subject: [PATCH] ekf2: only populate gnss pos aid src status if ref
 initialized

 - minor logging improvement when plotting the position from the
   beginning
---
 src/modules/ekf2/EKF/gps_control.cpp | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp
index 83670b5655d6..d86020815a8b 100644
--- a/src/modules/ekf2/EKF/gps_control.cpp
+++ b/src/modules/ekf2/EKF/gps_control.cpp
@@ -80,8 +80,11 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 			}
 		}
 
+		if (_pos_ref.isInitialized()) {
+			updateGnssPos(gnss_sample, _aid_src_gnss_pos);
+		}
+
 		updateGnssVel(gnss_sample, _aid_src_gnss_vel);
-		updateGnssPos(gnss_sample, _aid_src_gnss_pos);
 
 	} else if (_control_status.flags.gps) {
 		if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) {