Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Flow use parameter updates

Browse Source
This commit is contained in:
Paul Riseborough 2019-03-15 05:45:28 +11:00 committed by Andrew Tridgell
parent d3e9281846
commit 8038efc152
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_NavEKF2/AP_NavEKF2.cpp
View File

@ -35,7 +35,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
@ -61,7 +61,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
@ -87,7 +87,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 150 #define CHECK_SCALER_DEFAULT 150
#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
@ -113,7 +113,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
@ -557,15 +557,13 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
// @RebootRequired: True // @RebootRequired: True
AP_GROUPINFO("EXTNAV_DELAY", 50, NavEKF2, _extnavDelay_ms, 10), AP_GROUPINFO("EXTNAV_DELAY", 50, NavEKF2, _extnavDelay_ms, 10),
// @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", 51, NavEKF2, _flowUseMask, FLOW_USE_MASK_DEFAULT), AP_GROUPINFO("FLOW_USE", 51, NavEKF2, _flowUse, FLOW_USE_DEFAULT),
AP_GROUPEND AP_GROUPEND
}; };

7
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -399,7 +399,12 @@ private:
AP_Int8 _magMask; // Bitmask forcng specific EKF core instances to use simple heading magnetometer fusion. AP_Int8 _magMask; // Bitmask forcng specific EKF core instances to use simple heading magnetometer fusion.
AP_Int8 _originHgtMode; // Bitmask controlling post alignment correction and reporting of the EKF origin height. AP_Int8 _originHgtMode; // Bitmask controlling post alignment correction and reporting of the EKF origin height.
AP_Int8 _extnavDelay_ms; // effective average delay of external nav system measurements relative to inertial measurements (msec) AP_Int8 _extnavDelay_ms; // effective average delay of external nav system measurements relative to inertial measurements (msec)
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_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -170,7 +170,7 @@ void NavEKF2_core::setAidingMode()
bool canUseExtNav = readyToUseExtNav(); bool canUseExtNav = readyToUseExtNav();
if(canUseGPS || canUseRangeBeacon || canUseExtNav) { if(canUseGPS || canUseRangeBeacon || canUseExtNav) {
PV_AidingMode = AID_ABSOLUTE; PV_AidingMode = AID_ABSOLUTE;
} else if (optFlowDataPresent() && (frontend->_flowUseMask & (1<<0)) && filterIsStable) { } else if (optFlowDataPresent() && (frontend->_flowUse == FLOW_USE_NAV) && filterIsStable) {
PV_AidingMode = AID_RELATIVE; PV_AidingMode = AID_RELATIVE;
} }
} }

6
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -49,14 +49,14 @@ void NavEKF2_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
@ -86,7 +86,7 @@ void NavEKF2_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