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

AP_NavEKF3: Use parameter to set baro ground effect dead-zone

Browse Source
This commit is contained in:
Paul Riseborough 2021-05-31 17:21:58 +10:00 committed by Randy Mackay
parent 76d0dcc25c
commit 124f016e5b
3 changed files with 10 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -704,6 +704,14 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = {
// @User: Advanced
AP_GROUPINFO("OGNM_TEST_SF", 6, NavEKF3, _ognmTestScaleFactor, 2.0f),
// @Param: GND_EFF_DZ
// @DisplayName: Baro height ground effect dead zone
// @Description: This parameter sets the size of the dead zone that is applied to negative baro height spikes that can occur when takeing off or landing when a vehicle with lift rotors is operating in ground effect ground effect. Set to about 0.5m less than the amount of negative offset in baro height that occurs just prior to takeoff when lift motors are spooling up. Set to 0 if no ground effect is present.
// @Range: 0.0 10.0
// @Increment: 0.5
// @User: Advanced
AP_GROUPINFO("GND_EFF_DZ", 7, NavEKF3, _baroGndEffectDeadZone, 4.0f),
AP_GROUPEND
};

1
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -439,6 +439,7 @@ private:
AP_Float _momentumDragCoef; // lift rotor momentum drag coefficient
AP_Int8 _betaMask; // Bitmask controlling when sideslip angle fusion is used to estimate non wind states
AP_Float _ognmTestScaleFactor; // Scale factor applied to the thresholds used by the on ground not moving test
AP_Float _baroGndEffectDeadZone;// Dead zone applied to positive baro height innovations when in ground effect (m)
// Possible values for _flowUse
#define FLOW_USE_NONE 0

2
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -848,7 +848,7 @@ void NavEKF3_core::FuseVelPosNED()
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
} else if (obsIndex == 5) {
innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex];
const float gndMaxBaroErr = 4.0f;
const float gndMaxBaroErr = MAX(frontend->_baroGndEffectDeadZone, 0.0f);
const float gndBaroInnovFloor = -0.5f;
if ((dal.get_touchdown_expected() || dal.get_takeoff_expected()) && activeHgtSource == AP_NavEKF_Source::SourceZ::BARO) {