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

AP_NavEKF2: Allow threshold speed for range finder use to be adjusted

Browse Source
This commit is contained in:
priseborough 2016-10-05 22:09:36 +11:00 committed by Francisco Ferreira
parent 899fce53ff
commit 14a0155f5e
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_NavEKF2/AP_NavEKF2.cpp
View File

@ -517,6 +517,15 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
// @Units: msec
AP_GROUPINFO("BCN_DELAY", 46, NavEKF2, _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, NavEKF2, _useRngSwSpd, 2.0f),
AP_GROUPEND
};

3
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -356,11 +356,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_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -711,9 +711,11 @@ void NavEKF2_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) || !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