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

AP_NavEKF2: Enable simultaneous optical flow and GPS use

Browse Source 
Enables simultaneous use of GPS and optical flow data with automatic fallback to relative position mode if GPS is lost and automatic switch-up to absolute position status if GPS gained/re-gained.
This commit is contained in:
priseborough 2016-10-03 09:48:10 +11:00 committed by Randy Mackay
parent f135ca5ae7
commit f9018fcc1b
2 changed files with 7 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -178,7 +178,11 @@ void NavEKF2_core::setAidingMode()
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);
if (flowSensorTimeout || flowFusionTimeout) {
// 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) {

4
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -59,8 +59,8 @@ void NavEKF2_core::SelectFlowFusion()
EstimateTerrainOffset();
}
// Fuse optical flow data into the main filter if not excessively tilted and we are in the correct mode
if (flowDataToFuse && tiltOK && PV_AidingMode == AID_RELATIVE)
// Fuse optical flow data into the main filter
if (flowDataToFuse && tiltOK)
{
// Set the flow noise used by the fusion processes
R_LOS = sq(MAX(frontend->_flowNoise, 0.05f));