Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: flow use parameter improvements

Browse Source
This commit is contained in:
Paul Riseborough 2019-03-15 05:49:33 +11:00 committed by Andrew Tridgell
parent 8038efc152
commit 0d3e1a7d23
4 changed files with 17 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -34,7 +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
#define FLOW_USE_DEFAULT 1
#elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
// rover defaults
@ -59,7 +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
#define FLOW_USE_DEFAULT 1
#elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
// plane defaults
@ -84,7 +84,7 @@
#define FLOW_M_NSE_DEFAULT 0.15f
#define FLOW_I_GATE_DEFAULT 500
#define CHECK_SCALER_DEFAULT 100
#define FLOW_USE_MASK_DEFAULT 2
#define FLOW_USE_DEFAULT 2
#else
// build type not specified, use copter defaults
@ -109,7 +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
#define FLOW_USE_DEFAULT 1
#endif // APM_BUILD_DIRECTORY
@ -579,15 +579,13 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Units: m/s
AP_GROUPINFO("WENC_VERR", 53, NavEKF3, _wencOdmVelErr, 0.1f),
// @Param: FLOW_MASK
// @Param: FLOW_USE
// @DisplayName: Optical flow use bitmask
// @Description: Bitmask controlling if the optical flow data is fused into the 24-state navigation estimator OR the 1-state terrain height estimator.
// @Description: Controls if the optical flow data is fused into the 24-state navigation estimator OR the 1-state terrain height estimator.
// @User: Advanced
// @Values: 0:None,1:Navigation,2:Terrain
// @Bitmask: 0:Navigation,1:Terrain
// @Range: 0 2
// @RebootRequired: True
AP_GROUPINFO("FLOW_MASK", 54, NavEKF3, _flowUseMask, FLOW_USE_MASK_DEFAULT),
AP_GROUPINFO("FLOW_USE", 54, NavEKF3, _flowUse, FLOW_USE_DEFAULT),
AP_GROUPEND
};

7
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -419,7 +419,12 @@ 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.
AP_Int8 _flowUse; // Controls if the optical flow data is fused into the main navigation estimator and/or the terrain estimator.
// Possible values for _flowUse
#define FLOW_USE_NONE 0
#define FLOW_USE_NAV 1
#define FLOW_USE_TERRAIN 2
// 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() && (frontend->_flowUseMask & (1<<0))) || readyToUseBodyOdm()) {
} else if ((readyToUseOptFlow() && (frontend->_flowUse == FLOW_USE_NAV)) || readyToUseBodyOdm()) {
PV_AidingMode = AID_RELATIVE;
}
break;

6
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

@ -52,14 +52,14 @@ void NavEKF3_core::SelectFlowFusion()
}
// if have valid flow or range measurements, fuse data into a 1-state EKF to estimate terrain height
if (((flowDataToFuse && (frontend->_flowUseMask & (1<<1))) || rangeDataToFuse) && tiltOK) {
if (((flowDataToFuse && (frontend->_flowUse == FLOW_USE_TERRAIN)) || rangeDataToFuse) && tiltOK) {
// Estimate the terrain offset (runs a one state EKF)
EstimateTerrainOffset();
}
// Fuse optical flow data into the main filter
if (flowDataToFuse && tiltOK) {
if (frontend->_flowUseMask & (1<<0)) {
if (frontend->_flowUse == FLOW_USE_NAV) {
// 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
@ -89,7 +89,7 @@ void NavEKF3_core::EstimateTerrainOffset()
// don't fuse flow data if LOS rate is misaligned, without GPS, or insufficient velocity, as it is poorly observable
// don't fuse flow data if it exceeds validity limits
// don't update terrain offset if grpund is being used as the zero height datum in the main filter
bool cantFuseFlowData = (!(frontend->_flowUseMask & (1<<1))
bool cantFuseFlowData = ((frontend->_flowUse != FLOW_USE_TERRAIN)
|| gpsNotAvailable
|| PV_AidingMode == AID_RELATIVE
|| velHorizSq < 25.0f