From ca42df21f93e97b8acedd152c0759f9af903cb74 Mon Sep 17 00:00:00 2001 From: Clyde McQueen Date: Mon, 24 Jun 2024 09:47:36 -0700 Subject: [PATCH] Sub: request final yaw reset after diving 0.5m --- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 6 ++++++ libraries/AP_NavEKF3/AP_NavEKF3_core.h | 1 + 2 files changed, 7 insertions(+) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 402cfd97867bc..24861543bac86 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_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 2049e7278fad6..b6b21be2a0b3b 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