From 9a4108f55ea8c2f21407fed116f902576309c6e3 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 26 Jun 2020 13:29:57 +1000 Subject: [PATCH] AP_NavEKF3: Simplify setting EK3_MAG_CAL Don't require user to separately set EK3_MAG_CAL to fly without a magnetomer --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 4 ++-- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 9 ++++----- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 4 ++-- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 6 ++---- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 1 - 5 files changed, 10 insertions(+), 14 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 237013a7ad..dec36b0b2a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -244,8 +244,8 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Param: MAG_CAL // @DisplayName: Magnetometer default fusion mode - // @Description: This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states and when it will use a simpler magnetic heading fusion model that does not use magnetic field states. The 3-axis magnetometer fusion is only suitable for use when the external magnetic field environment is stable. EK3_MAG_CAL = 0 uses heading fusion on ground, 3-axis fusion in-flight, and is the default setting for Plane users. EK3_MAG_CAL = 1 uses 3-axis fusion only when manoeuvring. EK3_MAG_CAL = 2 uses heading fusion at all times, is recommended if the external magnetic field is varying and is the default for rovers. EK3_MAG_CAL = 3 uses heading fusion on the ground and 3-axis fusion after the first in-air field and yaw reset has completed, and is the default for copters. EK3_MAG_CAL = 4 uses 3-axis fusion at all times. EK3_MAG_CAL = 5 uses an external yaw sensor with simple heading fusion. NOTE : Use of simple heading magnetometer fusion makes vehicle compass calibration and alignment errors harder for the EKF to detect which reduces the sensitivity of the Copter EKF failsafe algorithm. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK3_MAG_MASK parameter. EK3_MAG_CAL = 6 uses an external yaw sensor with fallback to compass when the external sensor is not available if we are flying. EK3_MAG_CAL = 7 uses a yaw angle derived from IMU and GPS velocity data using a Gaussian Sum Filter (GSF). NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK3_MAG_MASK parameter. NOTE: When using EK3_MAG_CAL = 7, the EK3_GSF_RUN and EK3_GSF_USE masks must be set to the same as EK3_IMU_MASK and COMPASS_ENABLE must be set to 0 and yaw alignment will not occur until after takeoff with sufficient horizontal movement. - // @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always,5:Use external yaw sensor,6:External yaw sensor with compass fallback,7:Use GSF yaw estimate + // @Description: This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states and when it will use a simpler magnetic heading fusion model that does not use magnetic field states. The 3-axis magnetometer fusion is only suitable for use when the external magnetic field environment is stable. EK3_MAG_CAL = 0 uses heading fusion on ground, 3-axis fusion in-flight, and is the default setting for Plane users. EK3_MAG_CAL = 1 uses 3-axis fusion only when manoeuvring. EK3_MAG_CAL = 2 uses heading fusion at all times, is recommended if the external magnetic field is varying and is the default for rovers. EK3_MAG_CAL = 3 uses heading fusion on the ground and 3-axis fusion after the first in-air field and yaw reset has completed, and is the default for copters. EK3_MAG_CAL = 4 uses 3-axis fusion at all times. EK3_MAG_CAL = 5 uses an external yaw sensor with simple heading fusion. NOTE : Use of simple heading magnetometer fusion makes vehicle compass calibration and alignment errors harder for the EKF to detect which reduces the sensitivity of the Copter EKF failsafe algorithm. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK3_MAG_MASK parameter. EK3_MAG_CAL = 6 uses an external yaw sensor with fallback to compass when the external sensor is not available if we are flying. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK3_MAG_MASK parameter. NOTE: limited operation without a magnetometer or any other yaw sensor is possible by setting all COMPASS_USE, COMPASS_USE2, COMPASS_USE3, etc parameters to 0 and setting COMPASS_ENABLE to 0. If this is done, the EK3_GSF_RUN and EK3_GSF_USE masks must be set to the same as EK3_IMU_MASK. A yaw angle derived from IMU and GPS velocity data using a Gaussian Sum Filter (GSF) will then be used to align the yaw when flight commences and there is sufficient movement. + // @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always,5:Use external yaw sensor,6:External yaw sensor with compass fallback // @User: Advanced // @RebootRequired: True AP_GROUPINFO("MAG_CAL", 14, NavEKF3, _magCal, MAG_CAL_DEFAULT), diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index c09d05c889..df9b19c7a8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -45,7 +45,7 @@ NavEKF3_core::MagCal NavEKF3_core::effective_magCal(void) const MagCal magcal = MagCal(frontend->_magCal.get()); // force use of simple magnetic heading fusion for specified cores - if ((magcal != MagCal::EXTERNAL_YAW) && (magcal != MagCal::EXTERNAL_YAW_FALLBACK) && (magcal != MagCal::GSF_YAW) && (frontend->_magMask & core_index)) { + if ((magcal != MagCal::EXTERNAL_YAW) && (magcal != MagCal::EXTERNAL_YAW_FALLBACK) && (frontend->_magMask & core_index)) { return MagCal::NEVER; } @@ -286,7 +286,7 @@ void NavEKF3_core::setAidingMode() // This is a special case for when we are a fixed wing aircraft vehicle that has landed and // has no yaw measurement that works when static. Declare the yaw as unaligned (unknown) // and declare attitude aiding loss so that we fall back into a non-aiding mode - if (assume_zero_sideslip() && onGround && (effectiveMagCal == MagCal::GSF_YAW) && !use_compass()){ + if (assume_zero_sideslip() && onGround && !use_compass()){ yawAlignComplete = false; finalInflightYawInit = false; attAidLossCritical = true; @@ -479,7 +479,6 @@ bool NavEKF3_core::readyToUseExtNav(void) const bool NavEKF3_core::use_compass(void) const { return effectiveMagCal != MagCal::EXTERNAL_YAW && - effectiveMagCal != MagCal::GSF_YAW && _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw(magSelectIndex) && !allMagSensorsFailed; @@ -488,7 +487,7 @@ bool NavEKF3_core::use_compass(void) const // are we using a yaw source other than the magnetomer? bool NavEKF3_core::using_external_yaw(void) const { - if (effectiveMagCal == MagCal::EXTERNAL_YAW || effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK || effectiveMagCal == MagCal::GSF_YAW) { + if (effectiveMagCal == MagCal::EXTERNAL_YAW || effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK || !use_compass()) { return imuSampleTime_ms - last_gps_yaw_fusion_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000; } return false; @@ -553,7 +552,7 @@ void NavEKF3_core::checkGyroCalStatus(void) { // check delta angle bias variances const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg)); - if (effectiveMagCal == MagCal::GSF_YAW) { + if (!use_compass() && (effectiveMagCal != MagCal::EXTERNAL_YAW)) { // rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a compass // which can make this check fail Vector3f delAngBiasVarVec = Vector3f(P[10][10],P[11][11],P[12][12]); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index bcdab585ef..75a6d5a2d1 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -261,7 +261,7 @@ void NavEKF3_core::SelectMagFusion() // Handle case where we are not using a yaw sensor of any type and and attempt to reset the yaw in // flight using the output from the GSF yaw estimator. - if ((effectiveMagCal == MagCal::GSF_YAW || !use_compass()) && + if (!use_compass() && effectiveMagCal != MagCal::EXTERNAL_YAW && effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK) { @@ -1496,7 +1496,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw() EKFGSF_yaw_reset_ms = imuSampleTime_ms; EKFGSF_yaw_reset_count++; - if (effectiveMagCal == MagCal::GSF_YAW || AP::compass().get_num_enabled() == 0) { + if (!use_compass() || AP::compass().get_num_enabled() == 0) { gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned using GPS",(unsigned)imu_index); } else { gcs().send_text(MAV_SEVERITY_WARNING, "EKF3 IMU%u emergency yaw reset",(unsigned)imu_index); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 202d2b443e..af4cdbd0cc 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -1150,10 +1150,8 @@ float NavEKF3_core::MagDeclination(void) const */ void NavEKF3_core::updateMovementCheck(void) { - if (!onGround && - effectiveMagCal != MagCal::EXTERNAL_YAW && - effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK && - effectiveMagCal != MagCal::GSF_YAW) + const bool runCheck = onGround && (effectiveMagCal == MagCal::EXTERNAL_YAW || effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK || !use_compass()); + if (!runCheck) { onGroundNotMoving = false; return; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 3765ff2cce..a942973492 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -426,7 +426,6 @@ public: ALWAYS = 4, EXTERNAL_YAW = 5, EXTERNAL_YAW_FALLBACK = 6, - GSF_YAW = 7, }; // are we using an external yaw source? This is needed by AHRS attitudes_consistent check