diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 34dc0dfd66..335e76e9b9 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -210,7 +210,7 @@ void NavEKF3_core::setAidingMode() // GPS aiding is the preferred option unless excluded by the user if (readyToUseGPS() || readyToUseRangeBeacon() || readyToUseExtNav()) { PV_AidingMode = AID_ABSOLUTE; - } else if ((readyToUseOptFlow() && (frontend->_flowUse == FLOW_USE_NAV)) || readyToUseBodyOdm()) { + } else if (readyToUseOptFlow() || readyToUseBodyOdm()) { PV_AidingMode = AID_RELATIVE; } break; @@ -434,6 +434,15 @@ bool NavEKF3_core::useRngFinder(void) const // return true if the filter is ready to start using optical flow measurements bool NavEKF3_core::readyToUseOptFlow(void) const { + // ensure flow is used for navigation and not terrain alt estimation + if (frontend->_flowUse != FLOW_USE_NAV) { + return false; + } + + if (frontend->_sources.getVelXYSource() != AP_NavEKF_Source::SourceXY::OPTFLOW) { + return false; + } + // We need stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use optical flow return (imuSampleTime_ms - flowMeaTime_ms < 200) && tiltAlignComplete && delAngBiasLearned; } @@ -441,6 +450,11 @@ bool NavEKF3_core::readyToUseOptFlow(void) const // return true if the filter is ready to start using body frame odometry measurements bool NavEKF3_core::readyToUseBodyOdm(void) const { + if ((frontend->_sources.getVelXYSource() != AP_NavEKF_Source::SourceXY::EXTNAV) && + (frontend->_sources.getVelXYSource() != AP_NavEKF_Source::SourceXY::WHEEL_ENCODER)) { + return false; + } + // Check for fresh visual odometry data that meets the accuracy required for alignment bool visoDataGood = (imuSampleTime_ms - bodyOdmMeasTime_ms < 200) && (bodyOdmDataNew.velErr < 1.0f); @@ -467,12 +481,20 @@ bool NavEKF3_core::readyToUseGPS(void) const // return true if the filter to be ready to use the beacon range measurements bool NavEKF3_core::readyToUseRangeBeacon(void) const { + if (frontend->_sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::BEACON) { + return false; + } + return tiltAlignComplete && yawAlignComplete && delAngBiasLearned && rngBcnAlignmentCompleted && rngBcnDataToFuse; } // return true if the filter is ready to use external nav data bool NavEKF3_core::readyToUseExtNav(void) const { + if (frontend->_sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::EXTNAV) { + return false; + } + return tiltAlignComplete && extNavDataToFuse; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index a18a1cbc2c..78ac19919f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -817,7 +817,7 @@ private: // Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2 void calcIMU_Weighting(float K1, float K2); - // return true if the filter is ready to start using optical flow measurements + // return true if the filter is ready to start using optical flow measurements for position and velocity estimation bool readyToUseOptFlow(void) const; // return true if the filter is ready to start using body frame odometry measurements