From 0e6543f0e4b26fafca5d096e82e411ee0f04aa58 Mon Sep 17 00:00:00 2001
From: Clyde McQueen <clyde@mcqueen.net>
Date: Mon, 24 Jun 2024 09:47:36 -0700
Subject: [PATCH] AP_NavEKF3: sub update for yaw reset and inFlight checks

on sub, request final yaw reset after diving 0.5m.
Also update rangefinder tests for sub
---
 libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp     |  6 ++++++
 libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp | 12 ++++++++++++
 libraries/AP_NavEKF3/AP_NavEKF3_core.h            |  1 +
 3 files changed, 19 insertions(+)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
index 402cfd9786..24861543ba 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
@@ -62,9 +62,15 @@ void NavEKF3_core::controlMagYawReset()
     bool finalResetRequest = false;
     bool interimResetRequest = false;
     if (flightResetAllowed && !assume_zero_sideslip()) {
+#if APM_BUILD_TYPE(APM_BUILD_ArduSub)
+        // for sub, we'd like to be far enough away from metal structures like docks and vessels
+        // diving 0.5m is reasonable for both open water and pools
+        finalResetRequest = (stateStruct.position.z  - posDownAtTakeoff) > EKF3_MAG_FINAL_RESET_ALT_SUB;
+#else
         // check that we have reached a height where ground magnetic interference effects are insignificant
         // and can perform a final reset of the yaw and field states
         finalResetRequest = (stateStruct.position.z  - posDownAtTakeoff) < -EKF3_MAG_FINAL_RESET_ALT;
+#endif
 
         // check for increasing height
         bool hgtIncreasing = (posDownAtLastMagReset-stateStruct.position.z) > 0.5f;
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
index e6876b07a4..bb43be9883 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
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index 2049e7278f..b6b21be2a0 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -60,6 +60,7 @@
 
 // mag fusion final reset altitude (using NED frame so altitude is negative)
 #define EKF3_MAG_FINAL_RESET_ALT 2.5f
+#define EKF3_MAG_FINAL_RESET_ALT_SUB 0.5f
 
 // learning rate for mag biases when using GPS yaw
 #define EK3_GPS_MAG_LEARN_RATE 0.005f