Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: readyToUseOptFlow, Beacon, ExtNav check source

Browse Source
This commit is contained in:
Randy Mackay 2020-08-12 14:18:31 +09:00
parent faed58a027
commit 4cadaa5194
2 changed files with 24 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -210,7 +210,7 @@ void NavEKF3_core::setAidingMode()
// GPS aiding is the preferred option unless excluded by the user // GPS aiding is the preferred option unless excluded by the user
if (readyToUseGPS() || readyToUseRangeBeacon() || readyToUseExtNav()) { if (readyToUseGPS() || readyToUseRangeBeacon() || readyToUseExtNav()) {
PV_AidingMode = AID_ABSOLUTE; PV_AidingMode = AID_ABSOLUTE;
} else if ((readyToUseOptFlow() && (frontend->_flowUse == FLOW_USE_NAV)) || readyToUseBodyOdm()) { } else if (readyToUseOptFlow() || readyToUseBodyOdm()) {
PV_AidingMode = AID_RELATIVE; PV_AidingMode = AID_RELATIVE;
} }
break; break;
@ -434,6 +434,15 @@ bool NavEKF3_core::useRngFinder(void) const
// 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
bool NavEKF3_core::readyToUseOptFlow(void) const 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 // 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; 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 // return true if the filter is ready to start using body frame odometry measurements
bool NavEKF3_core::readyToUseBodyOdm(void) const 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 // Check for fresh visual odometry data that meets the accuracy required for alignment
bool visoDataGood = (imuSampleTime_ms - bodyOdmMeasTime_ms < 200) && (bodyOdmDataNew.velErr < 1.0f); 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 // return true if the filter to be ready to use the beacon range measurements
bool NavEKF3_core::readyToUseRangeBeacon(void) const bool NavEKF3_core::readyToUseRangeBeacon(void) const
{ {
if (frontend->_sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::BEACON) {
return false;
}
return tiltAlignComplete && yawAlignComplete && delAngBiasLearned && rngBcnAlignmentCompleted && rngBcnDataToFuse; return tiltAlignComplete && yawAlignComplete && delAngBiasLearned && rngBcnAlignmentCompleted && rngBcnDataToFuse;
} }
// return true if the filter is ready to use external nav data // return true if the filter is ready to use external nav data
bool NavEKF3_core::readyToUseExtNav(void) const bool NavEKF3_core::readyToUseExtNav(void) const
{ {
if (frontend->_sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::EXTNAV) {
return false;
}
return tiltAlignComplete && extNavDataToFuse; return tiltAlignComplete && extNavDataToFuse;
} }

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -817,7 +817,7 @@ private:
// Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2 // 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); 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; bool readyToUseOptFlow(void) const;
// return true if the filter is ready to start using body frame odometry measurements // return true if the filter is ready to start using body frame odometry measurements