ekf2: require valid filter vz for GPS vspeed check

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

@@ -189,15 +189,15 @@ bool Ekf::runGnssChecks(const gnssSample &gps)
 		// Apply a low pass filter
 		_gps_pos_deriv_filt = pos_derived * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef);
 
-		// Calculate the horizontal drift speed and fail if too high
+		// hdrift: calculate the horizontal drift speed and fail if too high
 		_gps_horizontal_position_drift_rate_m_s = Vector2f(_gps_pos_deriv_filt.xy()).norm();
 		_gps_check_fail_status.flags.hdrift = (_gps_horizontal_position_drift_rate_m_s > _params.req_hdrift);
 
-		// Fail if the vertical drift speed is too high
+		// vdrift: fail if the vertical drift speed is too high
 		_gps_vertical_position_drift_rate_m_s = fabsf(_gps_pos_deriv_filt(2));
 		_gps_check_fail_status.flags.vdrift = (_gps_vertical_position_drift_rate_m_s > _params.req_vdrift);
 
-		// Check the magnitude of the filtered horizontal GPS velocity
+		// hspeed: check the magnitude of the filtered horizontal GNSS velocity
 		const Vector2f gps_velNE = matrix::constrain(Vector2f(gps.vel.xy()),
 					   -10.0f * _params.req_hdrift,
 					   10.0f * _params.req_hdrift);
@@ -205,12 +205,20 @@ bool Ekf::runGnssChecks(const gnssSample &gps)
 		_gps_filtered_horizontal_velocity_m_s = _gps_velNE_filt.norm();
 		_gps_check_fail_status.flags.hspeed = (_gps_filtered_horizontal_velocity_m_s > _params.req_hdrift);
 
+		// vspeed: check the magnitude of the filtered vertical GNSS velocity
+		const float gnss_vz_limit = 10.f * _params.req_vdrift;
+		const float gnss_vz = math::constrain(gps.vel(2), -gnss_vz_limit, gnss_vz_limit);
+		_gps_vel_d_filt = gnss_vz * filter_coef + _gps_vel_d_filt * (1.f - filter_coef);
+
+		_gps_check_fail_status.flags.vspeed = (fabsf(_gps_vel_d_filt) > _params.req_vdrift);
+
 	} else if (_control_status.flags.in_air) {
 		// These checks are always declared as passed when flying
 		// If on ground and moving, the last result before movement commenced is kept
 		_gps_check_fail_status.flags.hdrift = false;
 		_gps_check_fail_status.flags.vdrift = false;
 		_gps_check_fail_status.flags.hspeed = false;
+		_gps_check_fail_status.flags.vspeed = false;
 
 		resetGpsDriftCheckFilters();
 
@@ -223,12 +231,6 @@ bool Ekf::runGnssChecks(const gnssSample &gps)
 	_gps_pos_prev.initReference(lat, lon, gps.time_us);
 	_gps_alt_prev = gps.alt;
 
-	// Check  the filtered difference between GPS and EKF vertical velocity
-	const float vz_diff_limit = 10.0f * _params.req_vdrift;
-	const float vertVel = math::constrain(gps.vel(2) - _state.vel(2), -vz_diff_limit, vz_diff_limit);
-	_gps_velD_diff_filt = vertVel * filter_coef + _gps_velD_diff_filt * (1.0f - filter_coef);
-	_gps_check_fail_status.flags.vspeed = (fabsf(_gps_velD_diff_filt) > _params.req_vdrift);
-
 	// assume failed first time through
 	if (_last_gps_fail_us == 0) {
 		_last_gps_fail_us = _time_delayed_us;
@@ -260,6 +262,8 @@ bool Ekf::runGnssChecks(const gnssSample &gps)
 void Ekf::resetGpsDriftCheckFilters()
 {
 	_gps_velNE_filt.setZero();
+	_gps_vel_d_filt = 0.f;
+
 	_gps_pos_deriv_filt.setZero();
 
 	_gps_horizontal_position_drift_rate_m_s = NAN;

src/modules/ekf2/EKF/ekf.h

@@ -652,7 +652,7 @@ private:
 	Vector3f _gps_pos_deriv_filt{};	///< GPS NED position derivative (m/sec)
 	Vector2f _gps_velNE_filt{};	///< filtered GPS North and East velocity (m/sec)
 
-	float _gps_velD_diff_filt{0.0f};	///< GPS filtered Down velocity (m/sec)
+	float _gps_vel_d_filt{0.0f};		///< GNSS filtered Down velocity (m/sec)
 	uint64_t _last_gps_fail_us{0};		///< last system time in usec that the GPS failed it's checks
 	uint64_t _last_gps_pass_us{0};		///< last system time in usec that the GPS passed it's checks
 	uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time