mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF3: skip GSF reset count check if source actively changed
also only fail all compass on emergency reset
This commit is contained in:
parent
ae725421bc
commit
c1b67d295f
|
@ -758,7 +758,7 @@ void NavEKF3_core::runYawEstimatorCorrection()
|
||||||
|
|
||||||
// action an external reset request
|
// 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) {
|
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 {
|
} else {
|
||||||
EKFGSF_yaw_valid_count = 0;
|
EKFGSF_yaw_valid_count = 0;
|
||||||
|
|
|
@ -150,7 +150,7 @@ void NavEKF3_core::realignYawGPS()
|
||||||
// correct yaw angle using GPS ground course if compass yaw bad
|
// correct yaw angle using GPS ground course if compass yaw bad
|
||||||
if (badMagYaw) {
|
if (badMagYaw) {
|
||||||
// attempt to use EKF-GSF estimate if available as it is more robust to GPS glitches
|
// attempt to use EKF-GSF estimate if available as it is more robust to GPS glitches
|
||||||
if (EKFGSF_resetMainFilterYaw()) {
|
if (EKFGSF_resetMainFilterYaw(true)) {
|
||||||
return;
|
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
|
// 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) {
|
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;
|
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
|
// 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
|
// Don't do a reset unless permitted by the EK3_GSF_USE and EK3_GSF_RUN parameter masks
|
||||||
if ((yawEstimator == nullptr)
|
if ((yawEstimator == nullptr)
|
||||||
|| !(frontend->_gsfUseMask & (1U<<core_index))
|
|| !(frontend->_gsfUseMask & (1U<<core_index))) {
|
||||||
|| EKFGSF_yaw_reset_count >= frontend->_gsfResetMaxCount) {
|
|
||||||
return false;
|
return false;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// limit the number of emergency resets
|
||||||
|
if (emergency_reset && (EKFGSF_yaw_reset_count >= frontend->_gsfResetMaxCount)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
ftype yawEKFGSF, yawVarianceEKFGSF;
|
ftype yawEKFGSF, yawVarianceEKFGSF;
|
||||||
if (EKFGSF_getYaw(yawEKFGSF, yawVarianceEKFGSF)) {
|
if (EKFGSF_getYaw(yawEKFGSF, yawVarianceEKFGSF)) {
|
||||||
// keep roll and pitch and reset yaw
|
// 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
|
// Fail the magnetomer so it doesn't get used and pull the yaw away from the correct value
|
||||||
|
if (emergency_reset) {
|
||||||
allMagSensorsFailed = true;
|
allMagSensorsFailed = true;
|
||||||
|
}
|
||||||
|
|
||||||
// record the yaw reset event
|
// record the yaw reset event
|
||||||
recordYawReset();
|
recordYawReset();
|
||||||
|
|
|
@ -951,8 +951,9 @@ private:
|
||||||
void resetQuatStateYawOnly(ftype yaw, ftype yawVariance, rotationOrder order);
|
void resetQuatStateYawOnly(ftype yaw, ftype yawVariance, rotationOrder order);
|
||||||
|
|
||||||
// attempt to reset the yaw to the EKF-GSF value
|
// 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
|
// 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)
|
// returns true on success and populates yaw (in radians) and yawVariance (rad^2)
|
||||||
bool EKFGSF_getYaw(ftype &yaw, ftype &yawVariance) const;
|
bool EKFGSF_getYaw(ftype &yaw, ftype &yawVariance) const;
|
||||||
|
|
Loading…
Reference in New Issue