From 80ee622f77371f6af1344310f2a5b99e3b87bf9b Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Wed, 10 Jul 2024 13:24:35 -0400
Subject: [PATCH] ekf2: baro only reset vz as a last resort

---
 .../ekf2/EKF/aid_sources/barometer/baro_height_control.cpp  | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp
index c6c6f220de..264c826738 100644
--- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp
+++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp
@@ -134,8 +134,10 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 					resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var);
 					bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
 
-					// reset vertical velocity
-					resetVerticalVelocityToZero();
+					// reset vertical velocity if no valid sources available
+					if (!isVerticalVelocityAidingActive()) {
+						resetVerticalVelocityToZero();
+					}
 
 					aid_src.time_last_fuse = imu_sample.time_us;