From 5ab17496f67ab633411c4c668aa8e104a6f00570 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 16 Apr 2023 10:14:31 +1000 Subject: [PATCH] AP_NavEKF3: Improve protection against GPS glitches during yaw alignment --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 1 + libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 41 ++++++++++++------- .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 1 + libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 1 + libraries/AP_NavEKF3/AP_NavEKF3_core.h | 10 +++++ 5 files changed, 39 insertions(+), 15 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 1eddc5951f..db32ae4682 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -231,6 +231,7 @@ void NavEKF3_core::setAidingMode() { PV_AidingMode = AID_NONE; yawAlignComplete = false; + yawAlignGpsValidCount = 0; finalInflightYawInit = false; ResetVelocity(resetDataSource::DEFAULT); ResetPosition(resetDataSource::DEFAULT); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 80f0ff35a4..330befc9d1 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -134,7 +134,7 @@ void NavEKF3_core::realignYawGPS(bool emergency_reset) Vector3F eulerAngles; stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z); - if (gpsDataDelayed.vel.xy().length_squared() > 25.0f) { + if (gpsDataDelayed.vel.xy().length_squared() > sq(GPS_VEL_YAW_ALIGN_MIN_SPD)) { // calculate course yaw angle ftype velYaw = atan2F(stateStruct.velocity.y,stateStruct.velocity.x); @@ -160,25 +160,36 @@ void NavEKF3_core::realignYawGPS(bool emergency_reset) const ftype gpsVelAcc = fmaxF(gpsSpdAccuracy, ftype(frontend->_gpsHorizVelNoise)); const ftype gps_yaw_variance = sq(asinF(constrain_float(gpsVelAcc/gpsDataDelayed.vel.xy().length(), -1.0F, 1.0F))); - // keep roll and pitch and reset yaw - rotationOrder order; - bestRotationOrder(order); - resetQuatStateYawOnly(gpsYaw, gps_yaw_variance, order); + if (gps_yaw_variance < sq(radians(GPS_VEL_YAW_ALIGN_MAX_ANG_ERR))) { + yawAlignGpsValidCount++; + } else { + yawAlignGpsValidCount = 0; + } - // reset the velocity and position states as they will be inaccurate due to bad yaw - ResetVelocity(resetDataSource::GPS); - ResetPosition(resetDataSource::GPS); + if (yawAlignGpsValidCount >= GPS_VEL_YAW_ALIGN_COUNT_THRESHOLD) { + yawAlignGpsValidCount = 0; + // keep roll and pitch and reset yaw + rotationOrder order; + bestRotationOrder(order); + resetQuatStateYawOnly(gpsYaw, gps_yaw_variance, order); - // send yaw alignment information to console - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned to GPS velocity",(unsigned)imu_index); + // reset the velocity and position states as they will be inaccurate due to bad yaw + ResetVelocity(resetDataSource::GPS); + ResetPosition(resetDataSource::GPS); - if (use_compass()) { - // request a mag field reset which may enable us to use the magnetometer if the previous fault was due to bad initialisation - magStateResetRequest = true; - // clear the all sensors failed status so that the magnetometers sensors get a second chance now that we are flying - allMagSensorsFailed = false; + // send yaw alignment information to console + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned to GPS velocity",(unsigned)imu_index); + + if (use_compass()) { + // request a mag field reset which may enable us to use the magnetometer if the previous fault was due to bad initialisation + magStateResetRequest = true; + // clear the all sensors failed status so that the magnetometers sensors get a second chance now that we are flying + allMagSensorsFailed = false; + } } } + } else { + yawAlignGpsValidCount = 0; } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index ce0a1322b4..fce8363ef8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -303,6 +303,7 @@ void NavEKF3_core::readMagData() // force a new yaw alignment 1s after learning completes. The // delay is to ensure any buffered mag samples are discarded yawAlignComplete = false; + yawAlignGpsValidCount = 0; InitialiseVariablesMag(); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 9fa9f5fd4f..65b9d02fda 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -281,6 +281,7 @@ void NavEKF3_core::InitialiseVariables() tiltErrorVariance = sq(M_2PI); tiltAlignComplete = false; yawAlignComplete = false; + yawAlignGpsValidCount = 0; have_table_earth_field = false; stateIndexLim = 23; last_gps_idx = 0; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 77a22d22bc..ba71bd1fd1 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -115,6 +115,15 @@ #define DOWNWARD_RANGEFINDER_MAX_INSTANCES 1 #endif +// 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 + class NavEKF3_core : public NavEKF_core_common { public: @@ -1116,6 +1125,7 @@ private: uint32_t lastYawReset_ms; // System time at which the last yaw reset occurred. Returned by getLastYawResetAngle bool tiltAlignComplete; // true when tilt alignment is complete bool yawAlignComplete; // true when yaw alignment is complete + uint8_t yawAlignGpsValidCount; // number of continuous good GPS velocity samples used for in flight yaw alignment bool magStateInitComplete; // true when the magnetic field states have been initialised uint8_t stateIndexLim; // Max state index used during matrix and array operations imu_elements imuDataDelayed; // IMU data at the fusion time horizon