From 6be06ecbb3b2585ebf261e809451b85bc8ad9c9d Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 27 Jun 2024 17:54:35 -0400
Subject: [PATCH] ekf2: optical flow failing also reset terrain if needed

---
 .../EKF/aid_sources/optical_flow/optical_flow_control.cpp     | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
index 4a6813eaae..299a9931cd 100644
--- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
+++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
@@ -145,6 +145,10 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 					if (is_flow_required) {
 						resetFlowFusion();
 
+						if (_hagl_sensor_status.flags.flow && !isTerrainEstimateValid()) {
+							resetTerrainToFlow();
+						}
+
 					} else {
 						stopFlowFusion();
 					}