diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 6ad9854626..c1e22686eb 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -243,8 +243,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. - // @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 + // @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 aligns the yaw angle using IMU and GPS velocity data. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK3_MAG_MASK parameter. NOTE: When using EKF3_MAG_CAL = 7, the EK3_GSF_RUN and EK3_GSF_USE masks must be set to the same as EK3_IMU_MASK and all COMPASS_USE parameters must be set to 0. + // @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 IMU and GPS velocity // @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 592a8b093f..fc08bed249 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 && (frontend->_magMask & core_index)) { + if (magcal != MagCal::EXTERNAL_YAW && magcal != MagCal::EXTERNAL_YAW_FALLBACK && magcal != MagCal::GSF_YAW && (frontend->_magMask & core_index)) { return MagCal::NEVER; } @@ -94,17 +94,16 @@ void NavEKF3_core::setWindMagStateLearningMode() } // Determine if learning of magnetic field states has been requested by the user - MagCal magCal = effective_magCal(); bool magCalRequested = - ((magCal == MagCal::WHEN_FLYING) && inFlight) || // when flying - ((magCal == MagCal::WHEN_MANOEUVRING) && manoeuvring) || // when manoeuvring - ((magCal == MagCal::AFTER_FIRST_CLIMB) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete - ((magCal == MagCal::EXTERNAL_YAW_FALLBACK) && inFlight) || - (magCal == MagCal::ALWAYS); // all the time + ((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, // we do not have an absolute position reference or are on the ground (unless explicitly requested by the user) - bool magCalDenied = !use_compass() || (magCal == MagCal::NEVER) || (onGround && magCal != MagCal::ALWAYS); + bool magCalDenied = !use_compass() || (effectiveMagCal == MagCal::NEVER) || (onGround && effectiveMagCal != MagCal::ALWAYS); // Inhibit the magnetic field calibration if not requested or denied bool setMagInhibit = !magCalRequested || magCalDenied; @@ -460,17 +459,20 @@ bool NavEKF3_core::readyToUseRangeBeacon(void) const // return true if we should use the compass bool NavEKF3_core::use_compass(void) const { - return effective_magCal() != MagCal::EXTERNAL_YAW && _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw(magSelectIndex) && !allMagSensorsFailed; + return effectiveMagCal != MagCal::EXTERNAL_YAW && + effectiveMagCal != MagCal::GSF_YAW && + _ahrs->get_compass() && + _ahrs->get_compass()->use_for_yaw(magSelectIndex) && + !allMagSensorsFailed; } -// are we using an external yaw source? Needed for ahrs attitudes_consistent +// are we using a yaw source other than the magnetomer? bool NavEKF3_core::using_external_yaw(void) const { - MagCal mag_cal = effective_magCal(); - if (mag_cal != MagCal::EXTERNAL_YAW && mag_cal != MagCal::EXTERNAL_YAW_FALLBACK) { - return false; + if (effectiveMagCal == MagCal::EXTERNAL_YAW || effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK || effectiveMagCal == MagCal::GSF_YAW) { + return AP_HAL::millis() - last_gps_yaw_fusion_ms < 5000 || AP_HAL::millis() - lastSynthYawTime_ms < 5000; } - return AP_HAL::millis() - last_gps_yaw_fusion_ms < 5000; + return false; } /* @@ -531,7 +533,7 @@ void NavEKF3_core::checkGyroCalStatus(void) { // check delta angle bias variances const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg)); - if (frontend->_magCal == 6) { + if (effective_magCal() == MagCal::GSF_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 0b2b7aab85..eaae2feebe 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -272,9 +272,58 @@ void NavEKF3_core::SelectMagFusion() // used for load levelling magFusePerformed = false; + effectiveMagCal = effective_magCal(); + + // 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 (!yawAlignComplete) { + yawAlignComplete = EKFGSF_resetMainFilterYaw(); + } + + if (imuSampleTime_ms - lastSynthYawTime_ms > 140) { + if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) { + // A 321 rotation order is best conditioned because the X axis is closer to horizontal than the Y axis + yawAngDataDelayed.type = 2; + } else { + // A 312 rotation order is best conditioned because the Y axis is closer to horizontal than the X axis + yawAngDataDelayed.type = 1; + } + + float yawEKFGSF, yawVarianceEKFGSF; + bool canUseEKFGSF = yawEstimator->getYawData(&yawEKFGSF, &yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(15.0f)); + if (yawAlignComplete && canUseEKFGSF) { + // use the EKF-GSF yaw estimator output as this is more robust that the EKF can achieve without a yaw measurement + yawAngDataDelayed.yawAngErr = MAX(sqrtf(yawVarianceEKFGSF), 0.05f); + yawAngDataDelayed.yawAng = yawEKFGSF; + fuseEulerYaw(false, true); + } else { + // fuse the last dead-reckoned yaw when static to stop yaw drift and estimate yaw gyro bias estimate + yawAngDataDelayed.yawAngErr = MAX(frontend->_yawNoise, 0.05f); + if (!onGroundNotMoving) { + if (yawAngDataDelayed.type == 2) { + yawAngDataDelayed.yawAng = atan2f(prevTnb[0][1], prevTnb[0][0]); + } else if (yawAngDataDelayed.type == 1) { + yawAngDataDelayed.yawAng = atan2f(-prevTnb[0][1], prevTnb[1][1]); + } + } + if (onGroundNotMoving) { + // fuse last known good yaw angle before we stopped moving to allow yaw bias learning when on ground before flight + fuseEulerYaw(false, true); + } else { + // prevent uncontrolled yaw variance growth by fusing a zero innovation + fuseEulerYaw(true, true); + } + } + magTestRatio.zero(); + yawTestRatio = 0.0f; + lastSynthYawTime_ms = imuSampleTime_ms; + } + return; + } + // Handle case where we are using an external yaw sensor instead of a magnetomer - MagCal magcal = effective_magCal(); - if (magcal == MagCal::EXTERNAL_YAW || magcal == MagCal::EXTERNAL_YAW_FALLBACK) { + if (effectiveMagCal == MagCal::EXTERNAL_YAW || effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK) { bool have_fused_gps_yaw = false; uint32_t now = AP_HAL::millis(); if (storedYawAng.recall(yawAngDataDelayed,imuDataDelayed.time_ms)) { @@ -310,7 +359,7 @@ void NavEKF3_core::SelectMagFusion() } lastSynthYawTime_ms = imuSampleTime_ms; } - if (magcal == MagCal::EXTERNAL_YAW) { + if (effectiveMagCal == MagCal::EXTERNAL_YAW) { // no fallback return; } @@ -350,35 +399,13 @@ void NavEKF3_core::SelectMagFusion() // fall through to magnetometer fusion } - if (magcal != MagCal::EXTERNAL_YAW_FALLBACK) { + if (effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK) { // check for and read new magnetometer measurements. We don't // real for EXTERNAL_YAW_FALLBACK as it has already been read // above readMagData(); } - // 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 (frontend->_magCal == 6 || !use_compass()) { - if (yawAlignComplete) { - // Fuse a synthetic heading measurement at 7Hz to prevent the filter covariances from - // becoming badly conditioned when static for long periods. For planes we only do this on-ground - // because they can align the yaw from GPS when airborne. For other platform types we do this - // all the time. - if ((onGround || !assume_zero_sideslip()) && (imuSampleTime_ms - lastSynthYawTime_ms > 140)) { - fuseEulerYaw(true, false); - magTestRatio.zero(); - yawTestRatio = 0.0f; - lastSynthYawTime_ms = imuSampleTime_ms; - } - return; - } - yawAlignComplete = EKFGSF_resetMainFilterYaw(); - return; - } - - // check for and read new magnetometer measurements - readMagData(); - + // If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout if (magHealth) { magTimeout = false; @@ -430,18 +457,6 @@ void NavEKF3_core::SelectMagFusion() } } - // If we have no magnetometer, fuse in a synthetic heading measurement at 7Hz to prevent the filter covariances - // from becoming badly conditioned. For planes we only do this on-ground because they can align the yaw from GPS when - // airborne. For other platform types we do this all the time. - if (!use_compass()) { - if ((onGround || !assume_zero_sideslip()) && (imuSampleTime_ms - lastSynthYawTime_ms > 140)) { - fuseEulerYaw(true, false); - magTestRatio.zero(); - yawTestRatio = 0.0f; - lastSynthYawTime_ms = imuSampleTime_ms; - } - } - // If the final yaw reset has been performed and the state variances are sufficiently low // record that the earth field has been learned. if (!magFieldLearned && finalInflightMagInit) { @@ -1410,8 +1425,6 @@ void NavEKF3_core::recordMagReset() yawInnovAtLastMagReset = innovYaw; } - - /* learn magnetometer biases from GPS yaw. Return true if the resulting mag vector is close enough to the one predicted by GPS @@ -1477,6 +1490,7 @@ bool NavEKF3_core::learnMagBiasFromGPS(void) } return ok; } + // Reset states using yaw from EKF-GSF and velocity and position from GPS bool NavEKF3_core::EKFGSF_resetMainFilterYaw() { @@ -1498,7 +1512,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw() EKFGSF_yaw_reset_ms = imuSampleTime_ms; EKFGSF_yaw_reset_count++; - if (frontend->_magCal == 6) { + if (effectiveMagCal == MagCal::GSF_YAW) { 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 - mag sensor stopped",(unsigned)imu_index); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 7f6a21aa6a..c2e579133f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -1063,12 +1063,15 @@ float NavEKF3_core::MagDeclination(void) const /* Update the on ground and not moving check. Should be called once per IMU update. - Only updates when on ground and when operating with an external yaw sensor + Only updates when on ground and when operating without a magnetometer */ void NavEKF3_core::updateMovementCheck(void) { - MagCal magcal = effective_magCal(); - if (!(magcal == MagCal::EXTERNAL_YAW || magcal == MagCal::EXTERNAL_YAW_FALLBACK) && !onGround) { + if (!onGround && + effectiveMagCal != MagCal::EXTERNAL_YAW && + effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK && + effectiveMagCal != MagCal::GSF_YAW) + { onGroundNotMoving = false; return; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 990db7b823..d85a3c2cae 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -607,10 +607,10 @@ void NavEKF3_core::send_status_report(mavlink_channel_t chan) const } else { temp = 0.0f; } - const float mag_max = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z); // send message - mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, mag_max, temp, tasVar); + mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, magVar.length(), temp, tasVar); + } // report the reason for why the backend is refusing to initialise diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index ec8eeb30c5..2fcdb05689 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -411,6 +411,8 @@ void NavEKF3_core::InitialiseVariables() EKFGSF_yaw_reset_request_ms = 0; EKFGSF_yaw_reset_count = 0; EKFGSF_run_filterbank = false; + + effectiveMagCal = effective_magCal(); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index b4566be2ed..6a7a8c2bc4 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -68,6 +68,7 @@ // learning limit for mag biases when using GPS yaw (Gauss) #define EK3_GPS_MAG_LEARN_LIMIT 0.02f + // maximum number of yaw resets due to detected magnetic anomaly allowed per flight #define MAG_ANOMALY_RESET_MAX 2 @@ -388,6 +389,7 @@ 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 @@ -951,6 +953,7 @@ private: 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 bool magFuseRequired; // boolean set to true when magnetometer fusion will be perfomred in the next time step + MagCal effectiveMagCal; // the actual mag calibration and yaw fusion method 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