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

AP_NavEKF3: Enable external control over use of GPS vertical velocity

Browse Source
This commit is contained in:
priseborough 2017-07-25 11:41:51 +10:00 committed by Andrew Tridgell
parent f2f3067326
commit 2310cb6d19
4 changed files with 13 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -609,7 +609,9 @@ NavEKF3::NavEKF3(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) :
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
sensorIntervalMin_ms(50), // The minimum allowed time between measurements from any non-IMU sensor (msec) sensorIntervalMin_ms(50), // The minimum allowed time between measurements from any non-IMU sensor (msec)
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);
} }

7
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -117,6 +117,10 @@ public:
// Returns 1 if command accepted // Returns 1 if command accepted
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;
@ -473,10 +477,11 @@ private:
} pos_down_reset_data; } pos_down_reset_data;
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 coreSetupRequired[7]; // true when this core index needs to be setup bool coreSetupRequired[7]; // true when this core index needs to be setup
uint8_t coreImuIndex[7]; // IMU index used by this core uint8_t coreImuIndex[7]; // IMU index used by this core
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

4
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -518,8 +518,8 @@ void NavEKF3_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, vertical velocity use is permitted 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;

4
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -176,7 +176,7 @@ void NavEKF3_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;
@ -499,7 +499,7 @@ void NavEKF3_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