aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: Prevent multiple aiding mode changes per update

Browse Source
This commit is contained in:
priseborough 2016-07-15 22:54:06 +10:00 committed by Andrew Tridgell
parent f4f347fb75
commit fcc07b5560
1 changed files with 3 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -160,7 +160,7 @@ void NavEKF2_core::setAidingMode()
// Save the previous status so we can detect when it has changed
PV_AidingModePrev = PV_AidingMode;
// Determine if we should start aiding
// 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
@ -172,10 +172,7 @@ void NavEKF2_core::setAidingMode()
} else if ((frontend->_fusionModeGPS == 3) && optFlowDataPresent()) {
PV_AidingMode = AID_RELATIVE;
}
}
// Determine if we should exit optical flow aiding
if (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)
@ -183,10 +180,7 @@ void NavEKF2_core::setAidingMode()
if (flowSensorTimeout || flowFusionTimeout) {
PV_AidingMode = AID_NONE;
}
}
// Determine if we should exit GPS aiding
if (PV_AidingMode == AID_ABSOLUTE) {
} else if (PV_AidingMode == AID_ABSOLUTE) {
// check if we can use opticalflow as a backup
bool optFlowBackupAvailable = (flowDataValid && !hgtTimeout);