Browse Source

AP_NavEKF3: Change the if statement to a switch statement.

master
murata 7 years ago committed by Francisco Ferreira
parent
commit
6a87840c7a
  1. 14
      libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

14
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@ -203,7 +203,8 @@ void NavEKF3_core::setAidingMode()
checkGyroCalStatus(); checkGyroCalStatus();
// Determine if we should change aiding mode // 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 // 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 // and IMU gyro bias estimates have stabilised
// If GPS usage has been prohiited then we use flow aiding provided optical flow data is present // If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
@ -213,7 +214,9 @@ void NavEKF3_core::setAidingMode()
} else if (readyToUseOptFlow() || readyToUseBodyOdm()) { } else if (readyToUseOptFlow() || readyToUseBodyOdm()) {
PV_AidingMode = AID_RELATIVE; PV_AidingMode = AID_RELATIVE;
} }
} else if (PV_AidingMode == AID_RELATIVE) { break;
}
case AID_RELATIVE: {
// Check if the fusion has timed out (flow measurements have been rejected for too long) // Check if the fusion has timed out (flow measurements have been rejected for too long)
bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000); bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
// Check if the fusion has timed out (body odometry measurements have been rejected for too long) // Check if the fusion has timed out (body odometry measurements have been rejected for too long)
@ -225,7 +228,9 @@ void NavEKF3_core::setAidingMode()
} else if (flowFusionTimeout && bodyOdmFusionTimeout) { } else if (flowFusionTimeout && bodyOdmFusionTimeout) {
PV_AidingMode = AID_NONE; PV_AidingMode = AID_NONE;
} }
} else if (PV_AidingMode == AID_ABSOLUTE) { break;
}
case AID_ABSOLUTE: {
// Find the minimum time without data required to trigger any check // 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)); uint16_t minTestTime_ms = MIN(frontend->tiltDriftTimeMax_ms, MIN(frontend->posRetryTimeNoVel_ms,frontend->posRetryTimeUseVel_ms));
@ -292,7 +297,8 @@ void NavEKF3_core::setAidingMode()
rngBcnTimeout = true; rngBcnTimeout = true;
gpsNotAvailable = true; gpsNotAvailable = true;
} }
break;
}
} }
// check to see if we are starting or stopping aiding and set states and modes as required // check to see if we are starting or stopping aiding and set states and modes as required

Loading…
Cancel
Save