Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 01:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Compensate for ground effect when takeoff or landing expected

Browse Source
This commit is contained in:
priseborough 2015-01-28 17:02:59 +11:00 committed by Andrew Tridgell
parent 20d92f5f9d
commit 3421a320b5
2 changed files with 53 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -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

16
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -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