From 124f016e5bd931b5a0a0b3908c12affab2ad3d34 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 31 May 2021 17:21:58 +1000 Subject: [PATCH] AP_NavEKF3: Use parameter to set baro ground effect dead-zone --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 8 ++++++++ libraries/AP_NavEKF3/AP_NavEKF3.h | 1 + libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 2 +- 3 files changed, 10 insertions(+), 1 deletion(-) 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) {