Skip to content

Commit

Permalink
AP_NavEKF3: Don't delay GPS use unnecessarily
Browse files Browse the repository at this point in the history
Fixed wing should not wait for bias state convergence after in-flight yaw alignment
  • Loading branch information
priseborough committed Jun 29, 2020
1 parent d2437d6 commit c5d2620
Showing 1 changed file with 3 additions and 3 deletions.
6 changes: 3 additions & 3 deletions libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit c5d2620

Please sign in to comment.