Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-26 18:48:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Add parameter to control how optical flow data is used

Browse Source
This commit is contained in:
Paul Riseborough 2019-02-23 07:52:22 +11:00 committed by Andrew Tridgell
parent aa6eee82f4
commit b848e231c7
4 changed files with 21 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -34,6 +34,7 @@
#define FLOW_M_NSE_DEFAULT 0.25f
#define FLOW_I_GATE_DEFAULT 300
#define CHECK_SCALER_DEFAULT 100
#define FLOW_USE_MASK_DEFAULT 1
#elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
// rover defaults
@ -58,6 +59,7 @@
#define FLOW_M_NSE_DEFAULT 0.25f
#define FLOW_I_GATE_DEFAULT 300
#define CHECK_SCALER_DEFAULT 100
#define FLOW_USE_MASK_DEFAULT 1
#elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
// plane defaults
@ -82,6 +84,7 @@
#define FLOW_M_NSE_DEFAULT 0.25f
#define FLOW_I_GATE_DEFAULT 300
#define CHECK_SCALER_DEFAULT 100
#define FLOW_USE_MASK_DEFAULT 2
#else
// build type not specified, use copter defaults
@ -106,6 +109,7 @@
#define FLOW_M_NSE_DEFAULT 0.25f
#define FLOW_I_GATE_DEFAULT 300
#define CHECK_SCALER_DEFAULT 100
#define FLOW_USE_MASK_DEFAULT 1
#endif // APM_BUILD_DIRECTORY
@ -575,6 +579,15 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Units: m/s
AP_GROUPINFO("WENC_VERR", 53, NavEKF3, _wencOdmVelErr, 0.1f),
// @Param: FLOW_MASK
// @DisplayName: Optical flow use bitmask
// @Description: Bitmask controlling if the optical flow data is fused into the 24-state navigation estimator and/or the 1-state terrain height estimator.
// @User: Advanced
// @Values: 0:None,1:Navgation,2:Terrain,3:Both
// @Bitmask: 0:Navigation,1:Terrain
// @RebootRequired: True
AP_GROUPINFO("FLOW_MASK", 54, NavEKF3, _flowUseMask, FLOW_USE_MASK_DEFAULT),
AP_GROUPEND
};

2
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -419,7 +419,7 @@ private:
AP_Float _visOdmVelErrMax; // Observation 1-STD velocity error assumed for visual odometry sensor at lowest reported quality (m/s)
AP_Float _visOdmVelErrMin; // Observation 1-STD velocity error assumed for visual odometry sensor at highest reported quality (m/s)
AP_Float _wencOdmVelErr; // Observation 1-STD velocity error assumed for wheel odometry sensor (m/s)
AP_Int8 _flowUseMask; // Bitmask controlling if the optical flow data is fused into the main navigation estimator and/or the terrain estimator.
// Tuning parameters
const float gpsNEVelVarAccScale = 0.05f; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration

2
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -209,7 +209,7 @@ void NavEKF3_core::setAidingMode()
// GPS aiding is the preferred option unless excluded by the user
if(readyToUseGPS() || readyToUseRangeBeacon()) {
PV_AidingMode = AID_ABSOLUTE;
} else if (readyToUseOptFlow() || readyToUseBodyOdm()) {
} else if ((readyToUseOptFlow() && (frontend->_flowUseMask & (1<<0))) || readyToUseBodyOdm()) {
PV_AidingMode = AID_RELATIVE;
}
break;

8
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

@ -57,16 +57,20 @@ void NavEKF3_core::SelectFlowFusion()
// fuse optical flow data into the terrain estimator if available and if there is no range data (range data is better)
fuseOptFlowData = (flowDataToFuse && !rangeDataToFuse);
// Estimate the terrain offset (runs a one state EKF)
EstimateTerrainOffset();
if (frontend->_flowUseMask & (1<<1)) {
EstimateTerrainOffset();
}
}
// Fuse optical flow data into the main filter
if (flowDataToFuse && tiltOK)
{
if (frontend->_flowUseMask & (1<<0)) {
// Set the flow noise used by the fusion processes
R_LOS = sq(MAX(frontend->_flowNoise, 0.05f));
// Fuse the optical flow X and Y axis data into the main filter sequentially
FuseOptFlow();
FuseOptFlow();
}
// reset flag to indicate that no new flow data is available for fusion
flowDataToFuse = false;
}