From 653030b038aa69b06fbaff0f98a2ea91903c5452 Mon Sep 17 00:00:00 2001 From: murata Date: Fri, 23 Dec 2016 09:31:33 +0900 Subject: [PATCH] AP_NavEKF2: Changed if statements to switch statement. AP_NavEKF2: Change indentation. --- libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp | 189 +++++++++++--------- 1 file changed, 104 insertions(+), 85 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index a5e90dc489..8e2fb32626 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -160,7 +160,8 @@ void NavEKF2_core::setAidingMode() PV_AidingModePrev = PV_AidingMode; // Determine if we should change aiding mode - if (PV_AidingMode == AID_NONE) { + 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 bool filterIsStable = tiltAlignComplete && yawAlignComplete && checkGyroCalStatus(); @@ -173,90 +174,101 @@ void NavEKF2_core::setAidingMode() } else if (optFlowDataPresent() && filterIsStable) { PV_AidingMode = AID_RELATIVE; } - } else if (PV_AidingMode == AID_RELATIVE) { - // Check if the optical flow sensor has timed out - bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000); - // Check if the fusion has timed out (flow measurements have been rejected for too long) - bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000); - // Enable switch to absolute position mode if GPS is available - // If GPS is not available and flow fusion has timed out, then fall-back to no-aiding - if((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit) { - PV_AidingMode = AID_ABSOLUTE; - } else if (flowSensorTimeout || flowFusionTimeout) { - 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 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; - - // 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 = 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) && - (imuSampleTime_ms - lastVelPassTime_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; + + case AID_RELATIVE: { + // Check if the optical flow sensor has timed out + bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000); + // Check if the fusion has timed out (flow measurements have been rejected for too long) + bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000); + // Enable switch to absolute position mode if GPS is available + // If GPS is not available and flow fusion has timed out, then fall-back to no-aiding + if((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit) { + PV_AidingMode = AID_ABSOLUTE; + } else if (flowSensorTimeout || flowFusionTimeout) { + 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 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; + + // 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 = 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) && + (imuSampleTime_ms - lastVelPassTime_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; + + default: + break; + } // check to see if we are starting or stopping aiding and set states and modes as required if (PV_AidingMode != PV_AidingModePrev) { // set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped. - if (PV_AidingMode == AID_NONE) { + switch (PV_AidingMode) { + case AID_NONE: // We have ceased aiding GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u has stopped aiding",(unsigned)imu_index); // When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors @@ -273,7 +285,9 @@ void NavEKF2_core::setAidingMode() meaHgtAtTakeOff = baroDataDelayed.hgt; // reset the vertical position state to faster recover from baro errors experienced during touchdown stateStruct.position.z = -meaHgtAtTakeOff; - } else if (PV_AidingMode == AID_RELATIVE) { + break; + + case AID_RELATIVE: // We have commenced aiding, but GPS usage has been prohibited so use optical flow only GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u is using optical flow",(unsigned)imu_index); posTimeout = true; @@ -282,7 +296,9 @@ void NavEKF2_core::setAidingMode() flowValidMeaTime_ms = imuSampleTime_ms; // Reset the last valid flow fusion time prevFlowFuseTime_ms = imuSampleTime_ms; - } else if (PV_AidingMode == AID_ABSOLUTE) { + break; + + case AID_ABSOLUTE: { bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit); bool canUseRangeBeacon = readyToUseRangeBeacon(); // We have commenced aiding and GPS usage is allowed @@ -301,14 +317,17 @@ void NavEKF2_core::setAidingMode() lastPosPassTime_ms = imuSampleTime_ms; lastVelPassTime_ms = imuSampleTime_ms; lastRngBcnPassTime_ms = imuSampleTime_ms; + } + break; + + default: + break; } // Always reset the position and velocity when changing mode ResetVelocity(); ResetPosition(); - } - } // Check the tilt and yaw alignmnent status