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

AP_NavEKF3: integrate AP_NavEKF_Source::useVelXYSource

Browse Source 
also integrate useVelZSource
This commit is contained in:
Randy Mackay 2020-11-13 09:59:40 +09:00
parent 72ee7d15e1
commit 6daaa06317
6 changed files with 15 additions and 15 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

@ -445,7 +445,7 @@ bool NavEKF3_core::readyToUseOptFlow(void) const
return false;
}
if (frontend->_sources.getVelXYSource() != AP_NavEKF_Source::SourceXY::OPTFLOW) {
if (!frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::OPTFLOW)) {
return false;
}
@ -456,8 +456,8 @@ 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)) {
if (!frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV) &&
!frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::WHEEL_ENCODER)) {
return false;
}
@ -664,7 +664,7 @@ void NavEKF3_core::runYawEstimatorPrediction()
// ensure GPS is used for horizontal position and velocity
if (frontend->_sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::GPS ||
frontend->_sources.getVelXYSource() != AP_NavEKF_Source::SourceXY::GPS) {
!frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS)) {
return;
}
@ -690,7 +690,7 @@ void NavEKF3_core::runYawEstimatorCorrection()
}
// ensure GPS is used for horizontal position and velocity
if (frontend->_sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::GPS ||
frontend->_sources.getVelXYSource() != AP_NavEKF_Source::SourceXY::GPS) {
!frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS)) {
return;
}

2
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -616,7 +616,7 @@ void NavEKF3_core::readGpsData()
}
// Check if GPS can output vertical velocity, vertical velocity use is permitted and set GPS fusion mode accordingly
if (gps.have_vertical_velocity(selected_gps) && (frontend->_sources.getVelZSource() == AP_NavEKF_Source::SourceZ::GPS) && !frontend->inhibitGpsVertVelUse) {
if (gps.have_vertical_velocity(selected_gps) && frontend->_sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) && !frontend->inhibitGpsVertVelUse) {
useGpsVertVel = true;
} else {
useGpsVertVel = false;

2
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

@ -52,7 +52,7 @@ void NavEKF3_core::SelectFlowFusion()
// Fuse optical flow data into the main filter
if (flowDataToFuse && tiltOK) {
if ((frontend->_flowUse == FLOW_USE_NAV) && (frontend->_sources.getVelXYSource() == AP_NavEKF_Source::SourceXY::OPTFLOW)) {
if ((frontend->_flowUse == FLOW_USE_NAV) && frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::OPTFLOW)) {
// Set the flow noise used by the fusion processes
R_LOS = sq(MAX(frontend->_flowNoise, 0.05f));
// Fuse the optical flow X and Y axis data into the main filter sequentially

2
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -119,7 +119,7 @@ bool NavEKF3_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, fl
bool NavEKF3_core::getHeightControlLimit(float &height) const
{
// only ask for limiting if we are doing optical flow navigation
if ((frontend->_sources.getVelXYSource() == AP_NavEKF_Source::SourceXY::OPTFLOW) && (PV_AidingMode == AID_RELATIVE) && flowDataValid) {
if (frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::OPTFLOW) && (PV_AidingMode == AID_RELATIVE) && flowDataValid) {
// If are doing optical flow nav, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors
const auto *_rng = dal.rangefinder();
if (_rng == nullptr) {

12
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -245,7 +245,7 @@ void NavEKF3_core::ResetHeight(void)
// Reset the vertical velocity state using GPS vertical velocity if we are airborne
// Check that GPS vertical velocity data is available and can be used
if (inFlight && !gpsNotAvailable && (frontend->_sources.getVelZSource() == AP_NavEKF_Source::SourceZ::GPS) && !frontend->inhibitGpsVertVelUse) {
if (inFlight && !gpsNotAvailable && frontend->_sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) && !frontend->inhibitGpsVertVelUse) {
stateStruct.velocity.z = gpsDataNew.vel.z;
} else if (inFlight && useExtNavVel && (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV)) {
stateStruct.velocity.z = extNavVelDelayed.vel.z;
@ -462,7 +462,7 @@ void NavEKF3_core::SelectVelPosFusion()
if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS)) {
// Don't fuse velocity data if GPS doesn't support it
fuseVelData = frontend->_sources.getVelXYSource() == AP_NavEKF_Source::SourceXY::GPS;
fuseVelData = frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS);
fusePosData = true;
extNavUsedForPos = false;
@ -485,7 +485,7 @@ void NavEKF3_core::SelectVelPosFusion()
// fuse external navigation velocity data if available
// extNavVelDelayed is already corrected for sensor position
if (extNavVelToFuse && (frontend->_sources.getVelXYSource() == AP_NavEKF_Source::SourceXY::EXTNAV)) {
if (extNavVelToFuse && frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV)) {
fuseVelData = true;
velPosObs[0] = extNavVelDelayed.vel.x;
velPosObs[1] = extNavVelDelayed.vel.y;
@ -720,7 +720,7 @@ void NavEKF3_core::FuseVelPosNED()
// test velocity measurements
uint8_t imax = 2;
// Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data
if ((frontend->_sources.getVelZSource() == AP_NavEKF_Source::SourceZ::NONE || PV_AidingMode != AID_ABSOLUTE || frontend->inhibitGpsVertVelUse) && !useExtNavVel) {
if ((!frontend->_sources.haveVelZSource() || PV_AidingMode != AID_ABSOLUTE || frontend->inhibitGpsVertVelUse) && !useExtNavVel) {
imax = 1;
}
float innovVelSumSq = 0; // sum of squares of velocity innovations
@ -1807,7 +1807,7 @@ void NavEKF3_core::SelectBodyOdomFusion()
// Check for body odometry data (aka visual position delta) at the fusion time horizon
const bool bodyOdomDataToFuse = storedBodyOdm.recall(bodyOdmDataDelayed, imuDataDelayed.time_ms);
if (bodyOdomDataToFuse && (frontend->_sources.getVelXYSource() == AP_NavEKF_Source::SourceXY::EXTNAV)) {
if (bodyOdomDataToFuse && frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV)) {
// Fuse data into the main filter
FuseBodyVel();
@ -1815,7 +1815,7 @@ void NavEKF3_core::SelectBodyOdomFusion()
// Check for wheel encoder data at the fusion time horizon
const bool wheelOdomDataToFuse = storedWheelOdm.recall(wheelOdmDataDelayed, imuDataDelayed.time_ms);
if (wheelOdomDataToFuse && (frontend->_sources.getVelXYSource() == AP_NavEKF_Source::SourceXY::WHEEL_ENCODER)) {
if (wheelOdomDataToFuse && frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::WHEEL_ENCODER)) {
// check if the delta time is too small to calculate a velocity
if (wheelOdmDataDelayed.delTime > EKF_TARGET_DT) {

2
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

@ -83,7 +83,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt;
gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f);
gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD);
} else if ((frontend->_sources.getVelZSource() == AP_NavEKF_Source::SourceZ::GPS) && !gps.have_vertical_velocity(preferred_gps)) {
} else if (frontend->_sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) && !gps.have_vertical_velocity(preferred_gps)) {
// If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail
gpsVertVelFail = true;
// if we have a 3D fix with no vertical velocity and