aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: Don't delay GPS use unnecessarily

Browse Source 
Fixed wing should not wait for bias state convergence after in-flight yaw alignment
This commit is contained in:
Paul Riseborough 2020-06-27 18:50:10 +10:00 committed by Peter Barker
parent 9a4108f55e
commit 24fccd5d87
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -460,7 +460,7 @@ bool NavEKF3_core::readyToUseBodyOdm(void) const
// return true if the filter to be ready to use gps // return true if the filter to be ready to use gps
bool NavEKF3_core::readyToUseGPS(void) const 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 // return true if the filter to be ready to use the beacon range measurements
@ -552,8 +552,8 @@ void NavEKF3_core::checkGyroCalStatus(void)
{ {
// check delta angle bias variances // check delta angle bias variances
const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg)); const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg));
if (!use_compass() && (effectiveMagCal != MagCal::EXTERNAL_YAW)) { 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 compass // 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 // which can make this check fail
Vector3f delAngBiasVarVec = Vector3f(P[10][10],P[11][11],P[12][12]); Vector3f delAngBiasVarVec = Vector3f(P[10][10],P[11][11],P[12][12]);
Vector3f temp = prevTnb * delAngBiasVarVec; Vector3f temp = prevTnb * delAngBiasVarVec;