Browse Source

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

zr-v5.1
Paul Riseborough 4 years ago committed by Randy Mackay
parent
commit
124f016e5b
  1. 8
      libraries/AP_NavEKF3/AP_NavEKF3.cpp
  2. 1
      libraries/AP_NavEKF3/AP_NavEKF3.h
  3. 2
      libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

8
libraries/AP_NavEKF3/AP_NavEKF3.cpp

@ -704,6 +704,14 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = { @@ -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

@ -439,6 +439,7 @@ private: @@ -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

@ -848,7 +848,7 @@ void NavEKF3_core::FuseVelPosNED() @@ -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) {

Loading…
Cancel
Save