Adds reset counter logic to EV height fusion

src/modules/ekf2/EKF/ekf.h

@@ -712,6 +712,7 @@ private:
 	void resetHeightToEv();
 
 	void resetVerticalVelocityToGps(const gpsSample &gps_sample);
+	void resetVerticalVelocityToEv(const extVisionSample &ev_sample);
 	void resetVerticalVelocityToZero();
 
 	// fuse optical flow line of sight rate measurements

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -259,6 +259,14 @@ void Ekf::resetVerticalVelocityToGps(const gpsSample &gps_sample)
 	P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_sample.sacc));
 }
 
+void Ekf::resetVerticalVelocityToEv(const extVisionSample &ev_sample)
+{
+	resetVerticalVelocityTo(ev_sample.vel(2));
+
+	// the state variance is the same as the observation
+	P.uncorrelateCovarianceSetVariance<1>(6, ev_sample.velVar(2));
+}
+
 void Ekf::resetVerticalVelocityToZero()
 {
 	// we don't know what the vertical velocity is, so set it to zero

src/modules/ekf2/EKF/ev_height_control.cpp

@@ -50,17 +50,24 @@ void Ekf::controlEvHeightFusion()
 	const bool ev_intermittent = !isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL);
 
 	if (_ev_data_ready) {
-		const bool continuing_conditions_passing = !ev_intermittent;
+		const bool position_valid = PX4_ISFINITE(_ev_sample_delayed.pos(2));
+		const bool continuing_conditions_passing = !ev_intermittent && position_valid;
 		const bool starting_conditions_passing = continuing_conditions_passing;
 
 		if (_control_status.flags.ev_hgt) {
 			if (continuing_conditions_passing) {
 				fuseEvHgt();
 
-				if (isHeightResetRequired()) {
+				const bool reset = (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter);
-					// All height sources are failing
+				if (isHeightResetRequired() || reset ) {
 					resetHeightToEv();
-					resetVerticalVelocityToZero();
+
+					// If the sample has a valid vertical velocity estimate, use it
+					if (PX4_ISFINITE(_ev_sample_delayed.vel(2))) {
+						resetVerticalVelocityToEv(_ev_sample_delayed);
+					} else {
+						resetVerticalVelocityToZero();
+					}
 				}
 
 			} else {