Sub: fix several EK3_MAG_Fusion cases for Sub

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;

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

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