Browse Source

AP_NavEKF3: fixed segfault when IMU mask covers more IMUs than GSF mask

c415-sdk
Andrew Tridgell 5 years ago
parent
commit
458ade86f5
  1. 3
      libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
  2. 2
      libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp

3
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

@ -246,7 +246,8 @@ void NavEKF3_core::SelectMagFusion() @@ -246,7 +246,8 @@ void NavEKF3_core::SelectMagFusion()
}
float yawEKFGSF, yawVarianceEKFGSF;
bool canUseEKFGSF = yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) &&
bool canUseEKFGSF = yawEstimator != nullptr &&
yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) &&
is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG));
if (yawAlignComplete && canUseEKFGSF && !assume_zero_sideslip()) {
// use the EKF-GSF yaw estimator output as this is more robust than the EKF can achieve without a yaw measurement

2
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp

@ -410,7 +410,7 @@ void NavEKF3_core::detectFlight() @@ -410,7 +410,7 @@ void NavEKF3_core::detectFlight()
if (!prevOnGround && onGround) {
// landed so disable filter bank
EKFGSF_run_filterbank = false;
} else if (!EKFGSF_run_filterbank && ((!prevInFlight && inFlight) || expectTakeoff)) {
} else if (yawEstimator && !EKFGSF_run_filterbank && ((!prevInFlight && inFlight) || expectTakeoff)) {
// started flying so reset counters and enable filter bank
EKFGSF_yaw_reset_ms = 0;
EKFGSF_yaw_reset_request_ms = 0;

Loading…
Cancel
Save