From f2f30673264a61c3d328aeb37a4e9fbf01008aad Mon Sep 17 00:00:00 2001 From: priseborough Date: Tue, 25 Jul 2017 11:35:03 +1000 Subject: [PATCH] AP_NavEKF2: Add interface to control GPS vertical velocity use --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 3 ++- libraries/AP_NavEKF2/AP_NavEKF2.h | 8 +++++++- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 4 ++-- libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 4 ++-- 4 files changed, 13 insertions(+), 6 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 819c0c2e68..9f7f461e0a 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -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); } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index cd682b5427..4423c39147 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -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; @@ -325,7 +329,7 @@ private: uint32_t _frameTimeUsec; // time per IMU frame uint8_t _framesPerPrediction; // expected number of IMU frames per prediction - + // EKF Mavlink Tuneable Parameters AP_Int8 _enable; // zero to disable EKF2 AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s @@ -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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index b26099427f..fcc4fa27fc 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -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; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index ffdca9d46b..92e6528689 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -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