AP_NavEKF2: setAidingMode uses lastExtNavPassTime_ms

This commit is contained in:
Randy Mackay 2020-04-18 10:52:51 +09:00
parent 3b20623a31
commit 014c889ceb
2 changed files with 12 additions and 4 deletions

View File

@ -209,14 +209,17 @@ void NavEKF2_core::setAidingMode()
bool posUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms);
bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms);
// Check if external nav is being used
bool extNavUsed = (imuSampleTime_ms - lastExtNavPassTime_ms <= minTestTime_ms);
// Check if attitude drift has been constrained by a measurement source
bool attAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed;
bool attAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed || extNavUsed;
// check if velocity drift has been constrained by a measurement source
bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed;
// check if position drift has been constrained by a measurement source
bool posAiding = posUsed || rngBcnUsed;
bool posAiding = posUsed || rngBcnUsed || extNavUsed;
// Check if the loss of attitude aiding has become critical
bool attAidLossCritical = false;
@ -225,6 +228,7 @@ void NavEKF2_core::setAidingMode()
(imuSampleTime_ms - lastTasPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastPosPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastExtNavPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms);
}
@ -239,8 +243,10 @@ void NavEKF2_core::setAidingMode()
maxLossTime_ms = frontend->posRetryTimeUseVel_ms;
}
posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) &&
(imuSampleTime_ms - lastExtNavPassTime_ms > maxLossTime_ms) &&
(imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms);
posAidLossPending = (imuSampleTime_ms - lastRngBcnPassTime_ms > (uint32_t)frontend->_gsfResetDelay) &&
(imuSampleTime_ms - lastExtNavPassTime_ms > (uint32_t)frontend->_gsfResetDelay) &&
(imuSampleTime_ms - lastPosPassTime_ms > (uint32_t)frontend->_gsfResetDelay);
}
@ -335,6 +341,7 @@ void NavEKF2_core::setAidingMode()
lastPosPassTime_ms = imuSampleTime_ms;
lastVelPassTime_ms = imuSampleTime_ms;
lastRngBcnPassTime_ms = imuSampleTime_ms;
lastExtNavPassTime_ms = imuSampleTime_ms;
}
break;
}

View File

@ -1160,6 +1160,7 @@ private:
ext_nav_elements extNavDataDelayed; // External nav at the fusion time horizon
uint32_t extNavMeasTime_ms; // time external measurements were accepted for input to the data buffer (msec)
uint32_t extNavLastPosResetTime_ms; // last time the external nav systen performed a position reset (msec)
uint32_t lastExtNavPassTime_ms; // time stamp when external nav position measurement last passed innovation consistency check (msec)
bool extNavDataToFuse; // true when there is new external nav data to fuse
bool extNavUsedForYaw; // true when the external nav data is also being used as a yaw observation
bool extNavUsedForPos; // true when the external nav data is being used as a position reference.