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

AP_NavEKF3: add EKFGSF_getYaw to reduce duplicate code

Browse Source
This commit is contained in:
Randy Mackay 2020-12-15 16:20:31 +09:00 committed by Peter Barker
parent c2edae905f
commit e819dbdd64
3 changed files with 28 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -690,14 +690,10 @@ void NavEKF3_core::runYawEstimatorCorrection()
float gpsVelAcc = fmaxf(gpsSpdAccuracy, frontend->_gpsHorizVelNoise);
yawEstimator->fuseVelData(gpsVelNE, gpsVelAcc);
// after velocity data has been fused the yaw variance esitmate will have been refreshed and
// after velocity data has been fused the yaw variance estimate will have been refreshed and
// is used maintain a history of validity
float yawEKFGSF, yawVarianceEKFGSF, velInnovLength;
bool canUseEKFGSF = yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) &&
is_positive(yawVarianceEKFGSF) &&
yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
(assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov));
if (canUseEKFGSF) {
float gsfYaw, gsfYawVariance;
if (EKFGSF_getYaw(gsfYaw, gsfYawVariance)) {
if (EKFGSF_yaw_valid_count < GSF_YAW_VALID_HISTORY_THRESHOLD) {
EKFGSF_yaw_valid_count++;
}

40
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -231,15 +231,10 @@ void NavEKF3_core::SelectMagFusion()
}
if (imuSampleTime_ms - lastSynthYawTime_ms > 140) {
float yawEKFGSF, yawVarianceEKFGSF, velInnovLength;
bool canUseEKFGSF = yawEstimator != nullptr &&
yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) &&
is_positive(yawVarianceEKFGSF) &&
yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
(assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov));
// use the EKF-GSF yaw estimator output as this is more robust than the EKF can achieve without a yaw measurement
// for non fixed wing platform types
const bool didUseEKFGSF = yawAlignComplete && canUseEKFGSF && !assume_zero_sideslip() && fuseEulerYaw(yawFusionMethod::GSF);
float gsfYaw, gsfYawVariance;
const bool didUseEKFGSF = yawAlignComplete && EKFGSF_getYaw(gsfYaw, gsfYawVariance) && !assume_zero_sideslip() && fuseEulerYaw(yawFusionMethod::GSF);
// fallback methods
if (!didUseEKFGSF) {
@ -853,17 +848,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
float gsfYaw, gsfYawVariance;
if (method == yawFusionMethod::GSF) {
bool canUseGsfYaw;
if (yawEstimator != nullptr) {
float velInnovLength;
canUseGsfYaw = yawEstimator->getYawData(gsfYaw, gsfYawVariance) &&
is_positive(gsfYawVariance) &&
gsfYawVariance < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
(assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov));
} else {
canUseGsfYaw = false;
}
if (!canUseGsfYaw) {
if (!EKFGSF_getYaw(gsfYaw, gsfYawVariance)) {
return false;
}
}
@ -1523,6 +1508,25 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
}
// returns true on success and populates yaw (in radians) and yawVariance (rad^2)
bool NavEKF3_core::EKFGSF_getYaw(float& yaw, float& yawVariance)
{
// return immediately if no yaw estimator
if (yawEstimator == nullptr) {
return false;
}
float velInnovLength;
if (yawEstimator->getYawData(yaw, yawVariance) &&
is_positive(yawVariance) &&
yawVariance < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
(assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov))) {
return true;
}
return false;
}
void NavEKF3_core::resetQuatStateYawOnly(float yaw, float yawVariance, rotationOrder order)
{
Quaternion quatBeforeReset = stateStruct.quat;

3
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -956,6 +956,9 @@ private:
// returns false if unsuccessful
bool EKFGSF_resetMainFilterYaw();
// returns true on success and populates yaw (in radians) and yawVariance (rad^2)
bool EKFGSF_getYaw(float &yaw, float &yawVariance);
// Fusion of body frame X and Y axis drag specific forces for multi-rotor wind estimation
void FuseDragForces();
void SelectDragFusion();