ekf2: GNSS velocity control should own vertical velocity reset if hei…

…ght faiing

 - GNSS height control using the velocity sample directly is ignoring
   potential position offsets

src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp

@@ -108,15 +108,6 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 					resetVerticalPositionTo(-(measurement - bias_est.getBias()), measurement_var);
 					bias_est.setBias(_state.pos(2) + measurement);
 
-					// reset vertical velocity
-					if (PX4_ISFINITE(gps_sample.vel(2)) && (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL))) {
-						// use 1.5 as a typical ratio of vacc/hacc
-						resetVerticalVelocityTo(gps_sample.vel(2), sq(math::max(1.5f * gps_sample.sacc, _params.gps_vel_noise)));
-
-					} else {
-						resetVerticalVelocityToZero();
-					}
-
 					aid_src.time_last_fuse = _time_delayed_us;
 
 				} else if (is_fusion_failing) {

src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp

@@ -132,17 +132,23 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 				}
 
 				if (do_vel_pos_reset) {
-					ECL_WARN("GPS fusion timeout, resetting velocity / position");
+					ECL_WARN("GPS fusion timeout, resetting");
+				}
 
-					if (gnss_vel_enabled) {
+				if (gnss_vel_enabled) {
+					if (do_vel_pos_reset) {
 						resetVelocityToGnss(_aid_src_gnss_vel);
-					}
 
-					if (gnss_pos_enabled) {
-						resetHorizontalPositionToGnss(_aid_src_gnss_pos);
+					} else if (isHeightResetRequired()) {
+						// reset vertical velocity if height is failing
+						resetVerticalVelocityTo(_aid_src_gnss_vel.observation[2], _aid_src_gnss_vel.observation_variance[2]);
 					}
 				}
 
+				if (gnss_pos_enabled && do_vel_pos_reset) {
+					resetHorizontalPositionToGnss(_aid_src_gnss_pos);
+				}
+
 			} else {
 				stopGpsFusion();
 			}