From cd6fd0ea4568d2f30ee41e876e6f94fa8aa25e08 Mon Sep 17 00:00:00 2001 From: priseborough Date: Tue, 13 Dec 2016 09:08:39 +1100 Subject: [PATCH] AP_NavEKF3: Improvements to range finder selection logic 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. --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 9 +++++++++ libraries/AP_NavEKF3/AP_NavEKF3.h | 3 ++- libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 6 ++++-- 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 2c11f3fe4e..4465a6d1d3 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -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 }; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 3a91ccf4d1..33d2d7e3ca 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index f65ef23d28..77faf87249 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -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