Browse Source

AP_NavEKF3: apply height innovation floor only when barometer is in use

mission-4.1.18
Jonathan Challinger 8 years ago committed by Tom Pittenger
parent
commit
c7a73e84d6
  1. 2
      libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

2
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@ -608,7 +608,7 @@ void NavEKF3_core::FuseVelPosNED() @@ -608,7 +608,7 @@ void NavEKF3_core::FuseVelPosNED()
const float gndMaxBaroErr = 4.0f;
const float gndBaroInnovFloor = -0.5f;
if(getTouchdownExpected()) {
if(getTouchdownExpected() && activeHgtSource == HGT_SOURCE_BARO) {
// when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor
// constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr
// this function looks like this:

Loading…
Cancel
Save