diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 80fce507ad..d205badff5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -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 }; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index bdf3c95449..6e93674780 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index f6bc6f24af..af630c694c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -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) {