From cfdaba35cca96808c0e6303391b42196cc76ff2d Mon Sep 17 00:00:00 2001
From: Charles Cross <charles@missionrobotics.us>
Date: Fri, 23 Sep 2022 09:52:45 -0700
Subject: [PATCH] Adds reset counter logic to EV height fusion

---
 src/modules/ekf2/EKF/ekf.h                 |  1 +
 src/modules/ekf2/EKF/ekf_helper.cpp        |  8 ++++++++
 src/modules/ekf2/EKF/ev_height_control.cpp | 15 +++++++++++----
 3 files changed, 20 insertions(+), 4 deletions(-)

diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index ff8a39c87c..99c8384f49 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/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
diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp
index 8ad9b5aca9..437f6291b4 100644
--- a/src/modules/ekf2/EKF/ekf_helper.cpp
+++ b/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
diff --git a/src/modules/ekf2/EKF/ev_height_control.cpp b/src/modules/ekf2/EKF/ev_height_control.cpp
index 73f6421cff..ad235d0caa 100644
--- a/src/modules/ekf2/EKF/ev_height_control.cpp
+++ b/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()) {
-					// All height sources are failing
+				const bool reset = (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter);
+				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 {