From 423c01973e18cc937a9c94289ab95008065809e5 Mon Sep 17 00:00:00 2001
From: Clyde McQueen <clyde@mcqueen.net>
Date: Mon, 24 Jun 2024 09:47:52 -0700
Subject: [PATCH] Sub: set inFlight to true if sub descends or rangefinder
 value decreases

---
 libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
index e6876b07a4484..bb43be98839ec 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
@@ -383,6 +383,17 @@ void NavEKF3_core::detectFlight()
         }
 
         if (!onGround) {
+#if APM_BUILD_TYPE(APM_BUILD_ArduSub)
+            // If depth has increased since arming, then we definitely are diving
+            if ((stateStruct.position.z - posDownAtTakeoff) > 1.5f) {
+                inFlight = true;
+            }
+
+            // If rangefinder has decreased since arming, then we definitely are diving
+            if ((rangeDataNew.rng - rngAtStartOfFlight) < -0.5f) {
+                inFlight = true;
+            }
+#else
             // If height has increased since exiting on-ground, then we definitely are flying
             if ((stateStruct.position.z - posDownAtTakeoff) < -1.5f) {
                 inFlight = true;
@@ -392,6 +403,7 @@ void NavEKF3_core::detectFlight()
             if ((rangeDataNew.rng - rngAtStartOfFlight) > 0.5f) {
                 inFlight = true;
             }
+#endif
 
             // If more than 5 seconds since likely_flying was set
             // true, then set inFlight true