From d64b2fcbf0d372114e279356b91ab96d07bc266a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 7 Jul 2017 17:05:04 +0900 Subject: [PATCH] AP_NavEKF3: final mag reset at 2.5m --- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 5d7c337be3..54936213e4 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -56,7 +56,7 @@ void NavEKF3_core::controlMagYawReset() if (flightResetAllowed && !assume_zero_sideslip()) { // 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) < -5.0f; + finalResetRequest = (stateStruct.position.z - posDownAtTakeoff) < -EKF3_MAG_FINAL_RESET_ALT; // 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 045f190c72..8be7273a45 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -61,6 +61,9 @@ #define EKF_TARGET_DT_MS 12 #define EKF_TARGET_DT 0.012f +// mag fusion final reset altitude (using NED frame so altitude is negative) +#define EKF3_MAG_FINAL_RESET_ALT 2.5f + class AP_AHRS; class NavEKF3_core