Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: improve ext nav glitch handling 

replaces extNavTimeout with posTimeout
replaces lastExtNavPassTime_ms with lastPosPassTime_ms
This commit is contained in:
Randy Mackay 2020-05-12 14:13:48 +09:00
parent b2dd6446c8
commit 5ad3611142
4 changed files with 6 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -247,21 +247,18 @@ void NavEKF3_core::setAidingMode()
// Check if range beacon data is being used
bool rngBcnUsed = (imuSampleTime_ms - lastRngBcnPassTime_ms <= minTestTime_ms);
// Check if GPS is being used
bool gpsPosUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms);
// Check if GPS or external nav is being used
bool posUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms);
bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms);
// Check if external nav position is being used
bool extNavUsed = (imuSampleTime_ms - lastExtNavPassTime_ms <= minTestTime_ms);
// Check if attitude drift has been constrained by a measurement source
bool attAiding = gpsPosUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed || bodyOdmUsed || extNavUsed;
bool attAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed || bodyOdmUsed;
// check if velocity drift has been constrained by a measurement source
bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed || bodyOdmUsed;
// check if position drift has been constrained by a measurement source
bool posAiding = gpsPosUsed || rngBcnUsed || extNavUsed;
bool posAiding = posUsed || rngBcnUsed;
// Check if the loss of attitude aiding has become critical
bool attAidLossCritical = false;
@ -270,7 +267,6 @@ void NavEKF3_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);
}
@ -284,7 +280,6 @@ void NavEKF3_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);
}
@ -296,14 +291,12 @@ void NavEKF3_core::setAidingMode()
rngBcnTimeout = true;
tasTimeout = true;
gpsNotAvailable = true;
extNavTimeout = true;
} else if (posAidLossCritical) {
// if the loss of position is critical, declare all sources of position aiding as being timed out
posTimeout = true;
velTimeout = true;
rngBcnTimeout = true;
gpsNotAvailable = true;
extNavTimeout = true;
}
break;
@ -385,7 +378,6 @@ void NavEKF3_core::setAidingMode()
lastPosPassTime_ms = imuSampleTime_ms;
lastVelPassTime_ms = imuSampleTime_ms;
lastRngBcnPassTime_ms = imuSampleTime_ms;
lastExtNavPassTime_ms = imuSampleTime_ms;
break;
}

4
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -128,8 +128,8 @@ void NavEKF3_core::ResetPosition(void)
// set the variances as received from external nav system data
P[7][7] = P[8][8] = sq(extNavDataDelayed.posErr);
// clear the timeout flags and counters
extNavTimeout = false;
lastExtNavPassTime_ms = imuSampleTime_ms;
posTimeout = false;
lastPosPassTime_ms = imuSampleTime_ms;
}
}
for (uint8_t i=0; i<imu_buffer_length; i++) {

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -398,10 +398,8 @@ void NavEKF3_core::InitialiseVariables()
extNavDataDelayed = {};
extNavMeasTime_ms = 0;
extNavLastPosResetTime_ms = 0;
lastExtNavPassTime_ms = 0;
extNavDataToFuse = false;
extNavUsedForPos = false;
extNavTimeout = false;
// zero data buffers
storedIMU.reset();

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1334,10 +1334,8 @@ 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 extNavUsedForPos; // true when the external nav data is being used as a position reference.
bool extNavTimeout; // true if external nav measurements have failed innovation consistency checks for too long
// flags indicating severe numerical errors in innovation variance calculation for different fusion operations
struct {