AP_NavEKF3: skip GSF reset count check if source actively changed

also only fail all compass on emergency reset
This commit is contained in:
Randy Mackay 2021-08-05 12:24:33 +09:00 committed by Andrew Tridgell
parent ae725421bc
commit c1b67d295f
3 changed files with 16 additions and 8 deletions

View File

@ -758,7 +758,7 @@ void NavEKF3_core::runYawEstimatorCorrection()
// action an external reset request
if (EKFGSF_yaw_reset_request_ms > 0 && imuSampleTime_ms - EKFGSF_yaw_reset_request_ms < YAW_RESET_TO_GSF_TIMEOUT_MS) {
EKFGSF_resetMainFilterYaw();
EKFGSF_resetMainFilterYaw(true);
}
} else {
EKFGSF_yaw_valid_count = 0;

View File

@ -150,7 +150,7 @@ void NavEKF3_core::realignYawGPS()
// correct yaw angle using GPS ground course if compass yaw bad
if (badMagYaw) {
// attempt to use EKF-GSF estimate if available as it is more robust to GPS glitches
if (EKFGSF_resetMainFilterYaw()) {
if (EKFGSF_resetMainFilterYaw(true)) {
return;
}
@ -229,7 +229,8 @@ void NavEKF3_core::SelectMagFusion()
// because this type of reset event is not as time critical, require a continuous history of valid estimates
if ((!yawAlignComplete || yaw_source_reset) && EKFGSF_yaw_valid_count >= GSF_YAW_VALID_HISTORY_THRESHOLD) {
yawAlignComplete = EKFGSF_resetMainFilterYaw();
const bool emergency_reset = (yaw_source != AP_NavEKF_Source::SourceYaw::GSF);
yawAlignComplete = EKFGSF_resetMainFilterYaw(emergency_reset);
yaw_source_reset = false;
}
@ -1531,15 +1532,19 @@ bool NavEKF3_core::learnMagBiasFromGPS(void)
}
// Reset states using yaw from EKF-GSF and velocity and position from GPS
bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
bool NavEKF3_core::EKFGSF_resetMainFilterYaw(bool emergency_reset)
{
// Don't do a reset unless permitted by the EK3_GSF_USE and EK3_GSF_RUN parameter masks
if ((yawEstimator == nullptr)
|| !(frontend->_gsfUseMask & (1U<<core_index))
|| EKFGSF_yaw_reset_count >= frontend->_gsfResetMaxCount) {
|| !(frontend->_gsfUseMask & (1U<<core_index))) {
return false;
};
// limit the number of emergency resets
if (emergency_reset && (EKFGSF_yaw_reset_count >= frontend->_gsfResetMaxCount)) {
return false;
}
ftype yawEKFGSF, yawVarianceEKFGSF;
if (EKFGSF_getYaw(yawEKFGSF, yawVarianceEKFGSF)) {
// keep roll and pitch and reset yaw
@ -1560,7 +1565,9 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
}
// Fail the magnetomer so it doesn't get used and pull the yaw away from the correct value
allMagSensorsFailed = true;
if (emergency_reset) {
allMagSensorsFailed = true;
}
// record the yaw reset event
recordYawReset();

View File

@ -951,8 +951,9 @@ private:
void resetQuatStateYawOnly(ftype yaw, ftype yawVariance, rotationOrder order);
// attempt to reset the yaw to the EKF-GSF value
// emergency_reset should be true if this reset is triggered by the loss of the yaw estimate
// returns false if unsuccessful
bool EKFGSF_resetMainFilterYaw();
bool EKFGSF_resetMainFilterYaw(bool emergency_reset);
// returns true on success and populates yaw (in radians) and yawVariance (rad^2)
bool EKFGSF_getYaw(ftype &yaw, ftype &yawVariance) const;