mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF2: Add interface to control GPS vertical velocity use
This commit is contained in:
parent
3642ecd9ef
commit
f2f3067326
|
@ -583,7 +583,8 @@ NavEKF2::NavEKF2(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) :
|
|||
gndEffectBaroScaler(4.0f), // scaler applied to the barometer observation variance when operating in ground effect
|
||||
gndGradientSigma(50), // RMS terrain gradient percentage assumed by the terrain height estimation
|
||||
fusionTimeStep_ms(10), // The minimum number of msec between covariance prediction and fusion operations
|
||||
runCoreSelection(false) // true when the default primary core has stabilised after startup and core selection can run
|
||||
runCoreSelection(false), // true when the default primary core has stabilised after startup and core selection can run
|
||||
inhibitGpsVertVelUse(false) // true when GPS vertical velocity use is prohibited
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
|
|
@ -122,6 +122,10 @@ public:
|
|||
// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
|
||||
uint8_t setInhibitGPS(void);
|
||||
|
||||
// Set the argument to true to prevent the EKF using the GPS vertical velocity
|
||||
// This can be used for situations where GPS velocity errors are causing problems with height accuracy
|
||||
void setGpsVertVelUse(const bool varIn) { inhibitGpsVertVelUse = varIn; };
|
||||
|
||||
// return the horizontal speed limit in m/s set by optical flow sensor limits
|
||||
// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
|
||||
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
|
||||
|
@ -439,6 +443,8 @@ private:
|
|||
|
||||
bool runCoreSelection; // true when the primary core has stabilised and the core selection logic can be started
|
||||
|
||||
bool inhibitGpsVertVelUse; // true when GPS vertical velocity use is prohibited
|
||||
|
||||
// update the yaw reset data to capture changes due to a lane switch
|
||||
// new_primary - index of the ekf instance that we are about to switch to as the primary
|
||||
// old_primary - index of the ekf instance that we are currently using as the primary
|
||||
|
|
|
@ -470,8 +470,8 @@ void NavEKF2_core::readGpsData()
|
|||
gpsNoiseScaler = 2.0f;
|
||||
}
|
||||
|
||||
// Check if GPS can output vertical velocity and set GPS fusion mode accordingly
|
||||
if (_ahrs->get_gps().have_vertical_velocity() && frontend->_fusionModeGPS == 0) {
|
||||
// Check if GPS can output vertical velocity, if it is allowed to be used, and set GPS fusion mode accordingly
|
||||
if (_ahrs->get_gps().have_vertical_velocity() && frontend->_fusionModeGPS == 0 && !frontend->inhibitGpsVertVelUse) {
|
||||
useGpsVertVel = true;
|
||||
} else {
|
||||
useGpsVertVel = false;
|
||||
|
|
|
@ -172,7 +172,7 @@ void NavEKF2_core::ResetHeight(void)
|
|||
|
||||
// Reset the vertical velocity state using GPS vertical velocity if we are airborne
|
||||
// Check that GPS vertical velocity data is available and can be used
|
||||
if (inFlight && !gpsNotAvailable && frontend->_fusionModeGPS == 0) {
|
||||
if (inFlight && !gpsNotAvailable && frontend->_fusionModeGPS == 0 && !frontend->inhibitGpsVertVelUse) {
|
||||
stateStruct.velocity.z = gpsDataNew.vel.z;
|
||||
} else if (onGround) {
|
||||
stateStruct.velocity.z = 0.0f;
|
||||
|
@ -495,7 +495,7 @@ void NavEKF2_core::FuseVelPosNED()
|
|||
// test velocity measurements
|
||||
uint8_t imax = 2;
|
||||
// Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data
|
||||
if (frontend->_fusionModeGPS >= 1 || PV_AidingMode != AID_ABSOLUTE) {
|
||||
if (frontend->_fusionModeGPS > 0 || PV_AidingMode != AID_ABSOLUTE || frontend->inhibitGpsVertVelUse) {
|
||||
imax = 1;
|
||||
}
|
||||
float innovVelSumSq = 0; // sum of squares of velocity innovations
|
||||
|
|
Loading…
Reference in New Issue