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(); }