Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-10 18:03:56 -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_M_NSE_DEFAULT 0.25f
#define FLOW_I_GATE_DEFAULT 300 #define FLOW_I_GATE_DEFAULT 300
#define CHECK_SCALER_DEFAULT 100 #define CHECK_SCALER_DEFAULT 100
#define FLOW_USE_MASK_DEFAULT 1 #define FLOW_USE_DEFAULT 1
#elif APM_BUILD_TYPE(APM_BUILD_APMrover2) #elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
// rover defaults // rover defaults
@ -59,7 +59,7 @@
#define FLOW_M_NSE_DEFAULT 0.25f #define FLOW_M_NSE_DEFAULT 0.25f
#define FLOW_I_GATE_DEFAULT 300 #define FLOW_I_GATE_DEFAULT 300
#define CHECK_SCALER_DEFAULT 100 #define CHECK_SCALER_DEFAULT 100
#define FLOW_USE_MASK_DEFAULT 1 #define FLOW_USE_DEFAULT 1
#elif APM_BUILD_TYPE(APM_BUILD_ArduPlane) #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
// plane defaults // plane defaults
@ -84,7 +84,7 @@
#define FLOW_M_NSE_DEFAULT 0.15f #define FLOW_M_NSE_DEFAULT 0.15f
#define FLOW_I_GATE_DEFAULT 500 #define FLOW_I_GATE_DEFAULT 500
#define CHECK_SCALER_DEFAULT 100 #define CHECK_SCALER_DEFAULT 100
#define FLOW_USE_MASK_DEFAULT 2 #define FLOW_USE_DEFAULT 2
#else #else
// build type not specified, use copter defaults // build type not specified, use copter defaults
@ -109,7 +109,7 @@
#define FLOW_M_NSE_DEFAULT 0.25f #define FLOW_M_NSE_DEFAULT 0.25f
#define FLOW_I_GATE_DEFAULT 300 #define FLOW_I_GATE_DEFAULT 300
#define CHECK_SCALER_DEFAULT 100 #define CHECK_SCALER_DEFAULT 100
#define FLOW_USE_MASK_DEFAULT 1 #define FLOW_USE_DEFAULT 1
#endif // APM_BUILD_DIRECTORY #endif // APM_BUILD_DIRECTORY
@ -579,15 +579,13 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Units: m/s // @Units: m/s
AP_GROUPINFO("WENC_VERR", 53, NavEKF3, _wencOdmVelErr, 0.1f), AP_GROUPINFO("WENC_VERR", 53, NavEKF3, _wencOdmVelErr, 0.1f),
// @Param: FLOW_MASK // @Param: FLOW_USE
// @DisplayName: Optical flow use bitmask // @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 // @User: Advanced
// @Values: 0:None,1:Navigation,2:Terrain // @Values: 0:None,1:Navigation,2:Terrain
// @Bitmask: 0:Navigation,1:Terrain
// @Range: 0 2
// @RebootRequired: True // @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 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 _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 _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_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 // Tuning parameters
const float gpsNEVelVarAccScale = 0.05f; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration 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 // GPS aiding is the preferred option unless excluded by the user
if(readyToUseGPS() || readyToUseRangeBeacon()) { if(readyToUseGPS() || readyToUseRangeBeacon()) {
PV_AidingMode = AID_ABSOLUTE; 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; PV_AidingMode = AID_RELATIVE;
} }
break; 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 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) // Estimate the terrain offset (runs a one state EKF)
EstimateTerrainOffset(); EstimateTerrainOffset();
} }
// Fuse optical flow data into the main filter // Fuse optical flow data into the main filter
if (flowDataToFuse && tiltOK) { if (flowDataToFuse && tiltOK) {
if (frontend->_flowUseMask & (1<<0)) { if (frontend->_flowUse == FLOW_USE_NAV) {
// Set the flow noise used by the fusion processes // Set the flow noise used by the fusion processes
R_LOS = sq(MAX(frontend->_flowNoise, 0.05f)); R_LOS = sq(MAX(frontend->_flowNoise, 0.05f));
// Fuse the optical flow X and Y axis data into the main filter sequentially // 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 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 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 // 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 || gpsNotAvailable
|| PV_AidingMode == AID_RELATIVE || PV_AidingMode == AID_RELATIVE
|| velHorizSq < 25.0f || velHorizSq < 25.0f