From faed58a027673908c2950217497f8bcc87a5f031 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 13 Jul 2020 14:40:03 +0900 Subject: [PATCH] AP_NavEKF3: integrate Source for yaw --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 23 +++++++++------ libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 29 +++++++++++++------ .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 3 +- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 3 +- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 9 +++--- 6 files changed, 43 insertions(+), 26 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 72195fa2f8..de66dddd82 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -233,7 +233,7 @@ 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. 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 + // @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always // @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 34a733e0d5..34dc0dfd66 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -38,14 +38,12 @@ void NavEKF3_core::controlFilterModes() */ 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) && (frontend->_magMask & core_index)) { + if (frontend->_magMask & core_index) { return MagCal::NEVER; } - return magcal; + return MagCal(frontend->_magCal.get()); } // Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to @@ -94,7 +92,6 @@ void NavEKF3_core::setWindMagStateLearningMode() ((effectiveMagCal == MagCal::WHEN_FLYING) && inFlight) || // when flying ((effectiveMagCal == MagCal::WHEN_MANOEUVRING) && manoeuvring) || // when manoeuvring ((effectiveMagCal == MagCal::AFTER_FIRST_CLIMB) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete - ((effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK) && inFlight) || (effectiveMagCal == MagCal::ALWAYS); // all the time // Deny mag calibration request if we aren't using the compass, it has been inhibited by the user, @@ -483,8 +480,14 @@ bool NavEKF3_core::readyToUseExtNav(void) const bool NavEKF3_core::use_compass(void) const { const auto *compass = dal.get_compass(); - return effectiveMagCal != MagCal::EXTERNAL_YAW && - compass && + const AP_NavEKF_Source::SourceYaw yaw_source = frontend->_sources.getYawSource(); + if ((yaw_source != AP_NavEKF_Source::SourceYaw::COMPASS) && + (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK)) { + // not using compass as a yaw source + return false; + } + + return compass && compass->use_for_yaw(magSelectIndex) && !allMagSensorsFailed; } @@ -492,7 +495,8 @@ 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 || !use_compass()) { + const AP_NavEKF_Source::SourceYaw yaw_source = frontend->_sources.getYawSource(); + if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK || !use_compass()) { return imuSampleTime_ms - last_gps_yaw_fusion_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000; } return false; @@ -558,7 +562,8 @@ void NavEKF3_core::checkGyroCalStatus(void) { // check delta angle bias variances const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg)); - if (!use_compass() && (effectiveMagCal != MagCal::EXTERNAL_YAW) && (effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK)) { + const AP_NavEKF_Source::SourceYaw yaw_source = frontend->_sources.getYawSource(); + if (!use_compass() && (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL) && (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK)) { // rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a yaw reference // 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 5aba14c9ae..3046a3269e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -199,17 +199,23 @@ void NavEKF3_core::SelectMagFusion() // used for load levelling magFusePerformed = false; - effectiveMagCal = effective_magCal(); + // get default yaw source + const AP_NavEKF_Source::SourceYaw yaw_source = frontend->_sources.getYawSource(); + if (yaw_source != yaw_source_last) { + yaw_source_last = yaw_source; + yaw_source_reset = true; + } // 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 (!use_compass() && - effectiveMagCal != MagCal::EXTERNAL_YAW && - effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK) { + yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL && + yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK) { // because this type of reset event is not as time critical, require a continuous history of valid estimates - if (!yawAlignComplete && EKFGSF_yaw_valid_count >= GSF_YAW_VALID_HISTORY_THRESHOLD) { + if ((!yawAlignComplete || yaw_source_reset) && EKFGSF_yaw_valid_count >= GSF_YAW_VALID_HISTORY_THRESHOLD) { yawAlignComplete = EKFGSF_resetMainFilterYaw(); + yaw_source_reset = false; } if (imuSampleTime_ms - lastSynthYawTime_ms > 140) { @@ -261,11 +267,12 @@ void NavEKF3_core::SelectMagFusion() } // Handle case where we are using an external yaw sensor instead of a magnetomer - if (effectiveMagCal == MagCal::EXTERNAL_YAW || effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK) { + if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK) { bool have_fused_gps_yaw = false; if (storedYawAng.recall(yawAngDataDelayed,imuDataDelayed.time_ms)) { - if (tiltAlignComplete && !yawAlignComplete) { + if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) { alignYawAngle(); + yaw_source_reset = false; } else if (tiltAlignComplete && yawAlignComplete) { fuseEulerYaw(false, true); } @@ -296,7 +303,7 @@ void NavEKF3_core::SelectMagFusion() } lastSynthYawTime_ms = imuSampleTime_ms; } - if (effectiveMagCal == MagCal::EXTERNAL_YAW) { + if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL) { // no fallback return; } @@ -344,9 +351,9 @@ void NavEKF3_core::SelectMagFusion() magTimeout = true; } - if (effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK) { + if (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK) { // check for and read new magnetometer measurements. We don't - // real for EXTERNAL_YAW_FALLBACK as it has already been read + // read for EXTERNAL_COMPASS_FALLBACK as it has already been read // above readMagData(); } @@ -356,6 +363,10 @@ void NavEKF3_core::SelectMagFusion() // Control reset of yaw and magnetic field states if we are using compass data if (magDataToFuse) { + if (yaw_source_reset && (yaw_source == AP_NavEKF_Source::SourceYaw::COMPASS)) { + magYawResetRequest = true; + yaw_source_reset = false; + } controlMagYawReset(); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 19f43b1eef..04ca4e5613 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -1279,7 +1279,8 @@ float NavEKF3_core::MagDeclination(void) const */ void NavEKF3_core::updateMovementCheck(void) { - const bool runCheck = onGround && (effectiveMagCal == MagCal::EXTERNAL_YAW || effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK || !use_compass()); + const AP_NavEKF_Source::SourceYaw yaw_source = frontend->_sources.getYawSource(); + const bool runCheck = onGround && (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK || !use_compass()); if (!runCheck) { onGroundNotMoving = false; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index cef871266c..9a43f33e8b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -534,8 +534,9 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void) ResetPosition(resetDataSource::DEFAULT); ResetHeight(); - // initialise the position source + // initialise sources pos_source_last = frontend->_sources.getPosXYSource(); + yaw_source_last = frontend->_sources.getYawSource(); // define Earth rotation vector in the NED navigation frame calcEarthRateNED(earthRateNED, dal.get_home().lat); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 60f9b29702..a18a1cbc2c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -418,9 +418,7 @@ public: WHEN_MANOEUVRING = 1, NEVER = 2, AFTER_FIRST_CLIMB = 3, - ALWAYS = 4, - EXTERNAL_YAW = 5, - EXTERNAL_YAW_FALLBACK = 6, + ALWAYS = 4 }; // are we using an external yaw source? This is needed by AHRS attitudes_consistent check @@ -1025,7 +1023,7 @@ private: ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements float defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive. bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step - MagCal effectiveMagCal; // the actual mag calibration and yaw fusion method being used as the default + MagCal effectiveMagCal; // the actual mag calibration being used as the default uint32_t prevTasStep_ms; // time stamp of last TAS fusion step uint32_t prevBetaStep_ms; // time stamp of last synthetic sideslip fusion step uint32_t lastMagUpdate_us; // last time compass was updated in usec @@ -1477,5 +1475,6 @@ private: // source reset handling AP_NavEKF_Source::SourceXY pos_source_last; // position source on previous iteration (used to detect a changes) bool pos_source_reset; // true when the position source has changed but the position has not yet been reset - + AP_NavEKF_Source::SourceYaw yaw_source_last; // yaw source on previous iteration (used to detect a change) + bool yaw_source_reset; // true when the yaw source has changed but the yaw has not yet been reset };