Browse Source

AP_NavEKF3: Don't delay GPS use unnecessarily

Fixed wing should not wait for bias state convergence after in-flight yaw alignment
zr-v5.1
Paul Riseborough 5 years ago committed by Peter Barker
parent
commit
24fccd5d87
  1. 6
      libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

6
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@ -460,7 +460,7 @@ bool NavEKF3_core::readyToUseBodyOdm(void) const @@ -460,7 +460,7 @@ bool NavEKF3_core::readyToUseBodyOdm(void) const
// return true if the filter to be ready to use gps
bool NavEKF3_core::readyToUseGPS(void) const
{
return validOrigin && tiltAlignComplete && yawAlignComplete && delAngBiasLearned && gpsGoodToAlign && (frontend->_fusionModeGPS != 3) && gpsDataToFuse && !gpsInhibit;
return validOrigin && tiltAlignComplete && yawAlignComplete && (delAngBiasLearned || assume_zero_sideslip()) && gpsGoodToAlign && (frontend->_fusionModeGPS != 3) && gpsDataToFuse && !gpsInhibit;
}
// return true if the filter to be ready to use the beacon range measurements
@ -552,8 +552,8 @@ void NavEKF3_core::checkGyroCalStatus(void) @@ -552,8 +552,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)) {
// rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a compass
if (!use_compass() && (effectiveMagCal != MagCal::EXTERNAL_YAW) && (effectiveMagCal != MagCal::EXTERNAL_YAW_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]);
Vector3f temp = prevTnb * delAngBiasVarVec;

Loading…
Cancel
Save