mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
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
|
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
|
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
|
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);
|
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
|
// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
|
||||||
uint8_t setInhibitGPS(void);
|
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 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
|
// 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;
|
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
|
||||||
@ -325,7 +329,7 @@ private:
|
|||||||
|
|
||||||
uint32_t _frameTimeUsec; // time per IMU frame
|
uint32_t _frameTimeUsec; // time per IMU frame
|
||||||
uint8_t _framesPerPrediction; // expected number of IMU frames per prediction
|
uint8_t _framesPerPrediction; // expected number of IMU frames per prediction
|
||||||
|
|
||||||
// EKF Mavlink Tuneable Parameters
|
// EKF Mavlink Tuneable Parameters
|
||||||
AP_Int8 _enable; // zero to disable EKF2
|
AP_Int8 _enable; // zero to disable EKF2
|
||||||
AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s
|
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 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
|
// 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
|
// 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
|
// 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;
|
gpsNoiseScaler = 2.0f;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Check if GPS can output vertical velocity and set GPS fusion mode accordingly
|
// 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) {
|
if (_ahrs->get_gps().have_vertical_velocity() && frontend->_fusionModeGPS == 0 && !frontend->inhibitGpsVertVelUse) {
|
||||||
useGpsVertVel = true;
|
useGpsVertVel = true;
|
||||||
} else {
|
} else {
|
||||||
useGpsVertVel = false;
|
useGpsVertVel = false;
|
||||||
|
@ -172,7 +172,7 @@ void NavEKF2_core::ResetHeight(void)
|
|||||||
|
|
||||||
// Reset the vertical velocity state using GPS vertical velocity if we are airborne
|
// 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
|
// 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;
|
stateStruct.velocity.z = gpsDataNew.vel.z;
|
||||||
} else if (onGround) {
|
} else if (onGround) {
|
||||||
stateStruct.velocity.z = 0.0f;
|
stateStruct.velocity.z = 0.0f;
|
||||||
@ -495,7 +495,7 @@ void NavEKF2_core::FuseVelPosNED()
|
|||||||
// test velocity measurements
|
// test velocity measurements
|
||||||
uint8_t imax = 2;
|
uint8_t imax = 2;
|
||||||
// Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data
|
// 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;
|
imax = 1;
|
||||||
}
|
}
|
||||||
float innovVelSumSq = 0; // sum of squares of velocity innovations
|
float innovVelSumSq = 0; // sum of squares of velocity innovations
|
||||||
|
Loading…
Reference in New Issue
Block a user