diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 6220c2f665..56a97469b2 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -401,7 +401,10 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) : DCM33FlowMin(0.71f), // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high. fScaleFactorPnoise(1e-10f), // Process noise added to focal length scale factor state variance at each time step flowTimeDeltaAvg_ms(100), // average interval between optical flow measurements (msec) - flowIntervalMax_ms(100) // maximum allowable time between flow fusion events + flowIntervalMax_ms(100), // maximum allowable time between flow fusion events + gndEffectTO_ms(30000), // time in msec that baro ground effect compensation will timeout after initiation + gndEffectBaroScaler(4.0f), // scaler applied to the barometer observation variance when operating in ground effect + gndEffectBaroTO_ms(5000) // time in msec that the baro measurement will be rejected if the gndEffectBaroVarLim has failed it #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN ,_perf_UpdateFilter(perf_alloc(PC_ELAPSED, "EKF_UpdateFilter")), @@ -1933,6 +1936,10 @@ void NavEKF::FuseVelPosNED() R_OBS[3] = sq(constrain_float(_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr); R_OBS[4] = R_OBS[3]; R_OBS[5] = sq(constrain_float(_baroAltNoise, 0.1f, 10.0f)); + // reduce weighting (increase observation noise) on baro if we are likely to be in ground effect + if (getGndEffectMode()) { + R_OBS[5] *= gndEffectBaroScaler; + } // if vertical GPS velocity data is being used, check to see if the GPS vertical velocity and barometer // innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting @@ -4727,4 +4734,33 @@ bool NavEKF::setOriginLLH(struct Location &loc) return true; } +// determine if the baro ground effect compensation is active +bool NavEKF::getGndEffectMode(void) +{ + if (gndEffectMode && (imuSampleTime_ms - startTimeTO_ms < gndEffectTO_ms) && gndEffectTO_ms != 0) { + gndEffectMode = true; + } else { + gndEffectMode = false; + startTimeTO_ms = 0; + } + return gndEffectMode; +} + + +// Tell the EKF to de-weight the baro sensor to take account of ground effect on baro during takeoff of landing when set to true +// Should be set to off by sending a false when the ground effect height region is cleared or the landing completed +// If not reactivated, this condition times out after the number of msec set by the _gndEffectTO_ms parameter +// The amount of de-weighting is controlled by the gndEffectBaroScaler parameter +void NavEKF::setGndEffectMode(bool setMode) +{ + if (setMode) { + startTimeTO_ms = imuSampleTime_ms; + gndEffectMode = true; + } else { + gndEffectMode = true; + startTimeTO_ms = 0; + } +} + + #endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 0454030c24..5dc15fc33c 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -194,6 +194,12 @@ public: // return data for debugging optical flow fusion void getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const; + // Tells the EKF to de-weight the baro sensor to take account of ground effect on baro during takeoff of landing when set to true + // Should be set to off by sending a false when the ground effect height region is cleared or the landing completed + // If not reactivated, this condition times out after the number of msec set by the _gndEffectTO_ms parameter + // The amount of de-weighting is controlled by the gndEffectBaroScaler parameter + void setGndEffectMode(bool modeOn); + /* return the filter fault status as a bitmasked integer 0 = quaternions are NaN @@ -405,6 +411,10 @@ private: // Set the NED origin to be used until the next filter reset void setOrigin(); + // get status of baro ground effect compensation mode + // returns true when baro ground effect compensation is required + bool getGndEffectMode(void); + // EKF Mavlink Tuneable Parameters AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s AP_Float _gpsVertVelNoise; // GPS vertical velocity measurement noise : m/s @@ -442,6 +452,9 @@ private: // Tuning parameters const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration const float gpsDVelVarAccScale; // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration + const uint16_t gndEffectTO_ms; // time in msec that ground effect mode is active after being activated + const float gndEffectBaroScaler; // scaler applied to the barometer observation variance when ground effect mode is active + const float gndEffectBaroTO_ms; // time in msec that the baro measurement will be rejected if the gndEffectBaroVarLim has failed it const float gpsPosVarAccScale; // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration const float msecHgtDelay; // Height measurement delay (msec) const uint16_t msecMagDelay; // Magnetometer measurement delay (msec) @@ -601,6 +614,9 @@ private: bool prevVehicleArmed; // vehicleArmed from previous frame struct Location EKF_origin; // LLH origin of the NED axis system - do not change unless filter is reset bool validOrigin; // true when the EKF origin is valid + bool gndEffectMode; // modifiy baro processing to compensate for ground effect + uint32_t startTimeTO_ms; // time in msec that the takeoff condition started - used to compensate for ground effect on baro height + uint32_t startTimeLAND_ms; // time in msec that the landing condition started - used to compensate for ground effect on baro height float gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the UBlox GPS receiver // Used by smoothing of state corrections