From 65b4db5edee9aa39a0cb09d874618d43bd8c45cf Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 3 Jul 2023 20:33:46 +0900 Subject: [PATCH] AP_NavEKF3: define Yaw alignment min GPS speed per vehicle --- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 7 +++++++ libraries/AP_NavEKF3/AP_NavEKF3_core.h | 3 --- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 330befc9d1..7d02dcec6f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -6,6 +6,13 @@ #include #include +// minimum GPS horizontal speed required to use GPS ground course for yaw alignment (m/s) +#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) + #define GPS_VEL_YAW_ALIGN_MIN_SPD 5.0F +#else + #define GPS_VEL_YAW_ALIGN_MIN_SPD 1.0F +#endif + /******************************************************** * RESET FUNCTIONS * ********************************************************/ diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index a59de0a82d..e1903a7faf 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -118,9 +118,6 @@ // number of continuous valid GPS velocity samples required to reset yaw #define GPS_VEL_YAW_ALIGN_COUNT_THRESHOLD 5 -// minimum GPS horizontal speed required to use GPS ground course for yaw alignment (m/s) -#define GPS_VEL_YAW_ALIGN_MIN_SPD 5.0F - // maximum GPs ground course uncertainty allowed for yaw alignment (deg) #define GPS_VEL_YAW_ALIGN_MAX_ANG_ERR 15.0F