aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: Improvements to range finder selection logic

Browse Source 
Enables the horizontal speed at which we switch from range finder to alternate to be adjusted.
Does not switch from range finder to alternate based on speed when speed estimate is invalid.
This commit is contained in:
priseborough 2016-12-13 09:08:39 +11:00 committed by Andrew Tridgell
parent 0e14992b34
commit cd6fd0ea45
3 changed files with 15 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -507,6 +507,15 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Units: msec
AP_GROUPINFO("BCN_DELAY", 46, NavEKF3, _rngBcnDelay_ms, 50),
// @Param: RNG_USE_SPD
// @DisplayName: Range finder max ground speed
// @Description: The range finder will not be used as the primary height source when the horizontal ground speed is greater than this value.
// @Range: 2.0 6.0
// @Increment: 0.5
// @User: Advanced
// @Units: m
AP_GROUPINFO("RNG_USE_SPD", 47, NavEKF3, _useRngSwSpd, 2.0f),
AP_GROUPEND
};

3
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -350,11 +350,12 @@ private:
AP_Float _yawNoise; // magnetic yaw measurement noise : rad
AP_Int16 _yawInnovGate; // Percentage number of standard deviations applied to magnetic yaw innovation consistency check
AP_Int8 _tauVelPosOutput; // Time constant of output complementary filter : csec (centi-seconds)
AP_Int8 _useRngSwHgt; // Maximum valid range of the range finder in metres
AP_Int8 _useRngSwHgt; // Maximum valid range of the range finder as a percentage of the maximum range specified by the sensor driver
AP_Float _terrGradMax; // Maximum terrain gradient below the vehicle
AP_Float _rngBcnNoise; // Range beacon measurement noise (m)
AP_Int16 _rngBcnInnovGate; // Percentage number of standard deviations applied to range beacon innovation consistency check
AP_Int8 _rngBcnDelay_ms; // effective average delay of range beacon measurements rel to IMU (msec)
AP_Float _useRngSwSpd; // Maximum horizontal ground speed to use range finder as the primary height source (m/s)
// Tuning parameters
const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration

6
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -699,9 +699,11 @@ void NavEKF3_core::selectHeightForFusion()
// If the terrain height is consistent and we are moving slowly, then it can be
// used as a height reference in combination with a range finder
// apply a hysteresis to the speed check to prevent rapid switching
float horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y);
bool dontTrustTerrain = (horizSpeed > 2.0f) || !terrainHgtStable;
bool trustTerrain = (horizSpeed < 1.0f) && terrainHgtStable;
bool dontTrustTerrain = ((horizSpeed > frontend->_useRngSwSpd) && filterStatus.flags.horiz_vel) || !terrainHgtStable;
float trust_spd_trigger = MAX((frontend->_useRngSwSpd - 1.0f),(frontend->_useRngSwSpd * 0.5f));
bool trustTerrain = (horizSpeed < trust_spd_trigger) && terrainHgtStable;
/*
* Switch between range finder and primary height source using height above ground and speed thresholds with