zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF3: Change the if statement to a switch statement.

master
murata 7 years ago committed by Francisco Ferreira
parent
a3e9f72b0b
commit
6a87840c7a
1 changed files with 96 additions and 90 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 186
      libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

186
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
Unescape View File

@ -203,97 +203,103 @@ void NavEKF3_core::setAidingMode() @@ -203,97 +203,103 @@ void NavEKF3_core::setAidingMode()
checkGyroCalStatus();
// Determine if we should change aiding mode
if (PV_AidingMode == AID_NONE) {
// Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
// and IMU gyro bias estimates have stabilised
// If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
// GPS aiding is the preferred option unless excluded by the user
if(readyToUseGPS() || readyToUseRangeBeacon()) {
PV_AidingMode = AID_ABSOLUTE;
} else if (readyToUseOptFlow() || readyToUseBodyOdm()) {
PV_AidingMode = AID_RELATIVE;
switch (PV_AidingMode) {
case AID_NONE: {
// Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
// and IMU gyro bias estimates have stabilised
// If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
// GPS aiding is the preferred option unless excluded by the user
if(readyToUseGPS() || readyToUseRangeBeacon()) {
PV_AidingMode = AID_ABSOLUTE;
} else if (readyToUseOptFlow() || readyToUseBodyOdm()) {
PV_AidingMode = AID_RELATIVE;
}
break;
}
case AID_RELATIVE: {
// Check if the fusion has timed out (flow measurements have been rejected for too long)
bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
// Check if the fusion has timed out (body odometry measurements have been rejected for too long)
bool bodyOdmFusionTimeout = ((imuSampleTime_ms - prevBodyVelFuseTime_ms) > 5000);
// Enable switch to absolute position mode if GPS or range beacon data is available
// If GPS or range beacons data is not available and flow fusion has timed out, then fall-back to no-aiding
if(readyToUseGPS() || readyToUseRangeBeacon()) {
PV_AidingMode = AID_ABSOLUTE;
} else if (flowFusionTimeout && bodyOdmFusionTimeout) {
PV_AidingMode = AID_NONE;
}
break;
}
case AID_ABSOLUTE: {
// Find the minimum time without data required to trigger any check
uint16_t minTestTime_ms = MIN(frontend->tiltDriftTimeMax_ms, MIN(frontend->posRetryTimeNoVel_ms,frontend->posRetryTimeUseVel_ms));
// Check if optical flow data is being used
bool optFlowUsed = (imuSampleTime_ms - prevFlowFuseTime_ms <= minTestTime_ms);
// Check if body odometry data is being used
bool bodyOdmUsed = (imuSampleTime_ms - prevBodyVelFuseTime_ms <= minTestTime_ms);
// Check if airspeed data is being used
bool airSpdUsed = (imuSampleTime_ms - lastTasPassTime_ms <= minTestTime_ms);
// 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);
bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms);
// Check if attitude drift has been constrained by a measurement source
bool attAiding = gpsPosUsed || 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;
// Check if the loss of attitude aiding has become critical
bool attAidLossCritical = false;
if (!attAiding) {
attAidLossCritical = (imuSampleTime_ms - prevFlowFuseTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastTasPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastPosPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms);
}
// Check if the loss of position accuracy has become critical
bool posAidLossCritical = false;
if (!posAiding ) {
uint16_t maxLossTime_ms;
if (!velAiding) {
maxLossTime_ms = frontend->posRetryTimeNoVel_ms;
} else {
maxLossTime_ms = frontend->posRetryTimeUseVel_ms;
}
posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) &&
(imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms);
}
if (attAidLossCritical) {
// if the loss of attitude data is critical, then put the filter into a constant position mode
PV_AidingMode = AID_NONE;
posTimeout = true;
velTimeout = true;
rngBcnTimeout = true;
tasTimeout = true;
gpsNotAvailable = 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;
}
break;
}
} else if (PV_AidingMode == AID_RELATIVE) {
// Check if the fusion has timed out (flow measurements have been rejected for too long)
bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
// Check if the fusion has timed out (body odometry measurements have been rejected for too long)
bool bodyOdmFusionTimeout = ((imuSampleTime_ms - prevBodyVelFuseTime_ms) > 5000);
// Enable switch to absolute position mode if GPS or range beacon data is available
// If GPS or range beacons data is not available and flow fusion has timed out, then fall-back to no-aiding
if(readyToUseGPS() || readyToUseRangeBeacon()) {
PV_AidingMode = AID_ABSOLUTE;
} else if (flowFusionTimeout && bodyOdmFusionTimeout) {
PV_AidingMode = AID_NONE;
}
} else if (PV_AidingMode == AID_ABSOLUTE) {
// Find the minimum time without data required to trigger any check
uint16_t minTestTime_ms = MIN(frontend->tiltDriftTimeMax_ms, MIN(frontend->posRetryTimeNoVel_ms,frontend->posRetryTimeUseVel_ms));
// Check if optical flow data is being used
bool optFlowUsed = (imuSampleTime_ms - prevFlowFuseTime_ms <= minTestTime_ms);
// Check if body odometry data is being used
bool bodyOdmUsed = (imuSampleTime_ms - prevBodyVelFuseTime_ms <= minTestTime_ms);
// Check if airspeed data is being used
bool airSpdUsed = (imuSampleTime_ms - lastTasPassTime_ms <= minTestTime_ms);
// 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);
bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms);
// Check if attitude drift has been constrained by a measurement source
bool attAiding = gpsPosUsed || 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;
// Check if the loss of attitude aiding has become critical
bool attAidLossCritical = false;
if (!attAiding) {
attAidLossCritical = (imuSampleTime_ms - prevFlowFuseTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastTasPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastPosPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms);
}
// Check if the loss of position accuracy has become critical
bool posAidLossCritical = false;
if (!posAiding ) {
uint16_t maxLossTime_ms;
if (!velAiding) {
maxLossTime_ms = frontend->posRetryTimeNoVel_ms;
} else {
maxLossTime_ms = frontend->posRetryTimeUseVel_ms;
}
posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) &&
(imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms);
}
if (attAidLossCritical) {
// if the loss of attitude data is critical, then put the filter into a constant position mode
PV_AidingMode = AID_NONE;
posTimeout = true;
velTimeout = true;
rngBcnTimeout = true;
tasTimeout = true;
gpsNotAvailable = 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;
}
}
}
// check to see if we are starting or stopping aiding and set states and modes as required
if (PV_AidingMode != PV_AidingModePrev) {

Write Preview
Loading…
Cancel
Save