Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Add interface to control GPS vertical velocity use

This commit is contained in:
priseborough 2017-07-25 11:35:03 +10:00 committed by Andrew Tridgell
parent 3642ecd9ef
commit f2f3067326
4 changed files with 13 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -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);
}

8
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

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

4
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

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

4
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

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