Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Fix integration of GPS yaw options 

AP_NavEKF3: fix failure to arm with EK3_MAG_CAL = 7

AP_NavEKF3: Fix failure to arm when not using magnetometer

AP_NavEKF3: Reduce yaw drift when EK3_MAG_CAL = 7
This commit is contained in:
Paul Riseborough 2020-04-11 10:43:46 +10:00 committed by Andrew Tridgell
parent 410b5825fb
commit 863f989130
7 changed files with 88 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -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),

32
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -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]);

98
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -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);

9
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -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;
}

4
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -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

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -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();
}

3
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -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