From 906731222db17418f5e23472f68f6cdc394cbc9e Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 16 May 2020 10:55:26 +1000 Subject: [PATCH] AP_NavEKF3: Fix range aid switching logic Clarifies use of EK3_RANGE_USE_HGT and EK3_ALT_SOURCE parameters to control range finder use. Setting EK3_RNG_USE_HGT to a positive number to enable automatic switching between the primary height source (baro or GPS) and range finder should be disabled if any primary height source other than Baro or GPS is selected via the EK3_ALT_SOURCE parameter. --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 4 +- .../AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 80 +++++++++---------- 2 files changed, 41 insertions(+), 43 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index f57120a64d..f2555eb84a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -197,7 +197,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Param: ALT_SOURCE // @DisplayName: Primary altitude sensor source - // @Description: Primary height sensor used by the EKF. If the selected option cannot be used, baro is used. 1 uses the range finder and only with optical flow navigation (EK3_GPS_TYPE = 3), Do not use "1" for terrain following. NOTE: the EK3_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground. Setting 4 uses external nav system data, but only if data is also being used for horizontal position + // @Description: Primary height sensor used by the EKF. If a sensor other than Baro is selected and becomes unavailable, then the Baro sensor will be used as a fallback. NOTE: the EK3_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground in conjunction with EK3_ALT_SOURCE = 0 or 2 (Baro or GPS). NOTE: Setting EK3_ALT_SOURCE = 4 uses external nav system data only if data is also being used for horizontal position // @Values: 0:Use Baro, 1:Use Range Finder, 2:Use GPS, 3:Use Range Beacon, 4:Use External Nav // @User: Advanced // @RebootRequired: True @@ -477,7 +477,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Param: RNG_USE_HGT // @DisplayName: Range finder switch height percentage - // @Description: Range finder can be used as the primary height source when below this percentage of its maximum range (see RNGFND_MAX_CM). Set to -1 when EK3_ALT_SOURCE is not set to range finder. This is not for terrain following. + // @Description: Range finder can be used as the primary height source when below this percentage of its maximum range (see RNGFND_MAX_CM). This will not work unless Baro or GPS height is selected as the primary height source vis EK3_ALT_SOURCE = 0 or 2 respectively. This feature should not be used for terrain following as it is designed for vertical takeoff and landing with climb above the range finder use height before commencing the mission, and with horizontal position changes below that height being limited to a flat region around the takeoff and landing point. // @Range: -1 70 // @Increment: 1 // @User: Advanced diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 6ee8a90396..53fbe9e11c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -981,55 +981,53 @@ void NavEKF3_core::selectHeightForFusion() if (extNavUsedForPos && (frontend->_altSource == 4)) { // always use external navigation as the height source if using for position. activeHgtSource = HGT_SOURCE_EXTNAV; - } else if (_rng && ((frontend->_useRngSwHgt > 0) && (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) { - if (frontend->_altSource == 1) { - // always use range finder - activeHgtSource = HGT_SOURCE_RNG; + } else if ((frontend->_altSource == 1) && _rng && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) { + // user has specified the range finder as a primary height source + activeHgtSource = HGT_SOURCE_RNG; + } else if ((frontend->_useRngSwHgt > 0) && ((frontend->_altSource == 0) || (frontend->_altSource == 2)) && _rng && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) { + // determine if we are above or below the height switch region + float rangeMaxUse = 1e-4f * (float)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (float)frontend->_useRngSwHgt; + bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse; + bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse; + + // 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 + bool dontTrustTerrain, trustTerrain; + if (filterStatus.flags.horiz_vel) { + // We can use the velocity estimate + float horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y); + dontTrustTerrain = (horizSpeed > frontend->_useRngSwSpd) || !terrainHgtStable; + float trust_spd_trigger = MAX((frontend->_useRngSwSpd - 1.0f),(frontend->_useRngSwSpd * 0.5f)); + trustTerrain = (horizSpeed < trust_spd_trigger) && terrainHgtStable; } else { - // determine if we are above or below the height switch region - float rangeMaxUse = 1e-4f * (float)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (float)frontend->_useRngSwHgt; - bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse; - bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse; - - // 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 - bool dontTrustTerrain, trustTerrain; - if (filterStatus.flags.horiz_vel) { - // We can use the velocity estimate - float horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y); - dontTrustTerrain = (horizSpeed > frontend->_useRngSwSpd) || !terrainHgtStable; - float trust_spd_trigger = MAX((frontend->_useRngSwSpd - 1.0f),(frontend->_useRngSwSpd * 0.5f)); - trustTerrain = (horizSpeed < trust_spd_trigger) && terrainHgtStable; - } else { - // We can't use the velocity estimate - dontTrustTerrain = !terrainHgtStable; - trustTerrain = terrainHgtStable; - } + // We can't use the velocity estimate + dontTrustTerrain = !terrainHgtStable; + trustTerrain = terrainHgtStable; + } - /* - * Switch between range finder and primary height source using height above ground and speed thresholds with - * hysteresis to avoid rapid switching. Using range finder for height requires a consistent terrain height - * which cannot be assumed if the vehicle is moving horizontally. - */ - if ((aboveUpperSwHgt || dontTrustTerrain) && (activeHgtSource == HGT_SOURCE_RNG)) { - // cannot trust terrain or range finder so stop using range finder height - if (frontend->_altSource == 0) { - activeHgtSource = HGT_SOURCE_BARO; - } else if (frontend->_altSource == 2) { - activeHgtSource = HGT_SOURCE_GPS; - } - } else if (belowLowerSwHgt && trustTerrain && (activeHgtSource != HGT_SOURCE_RNG)) { - // reliable terrain and range finder so start using range finder height - activeHgtSource = HGT_SOURCE_RNG; + /* + * Switch between range finder and primary height source using height above ground and speed thresholds with + * hysteresis to avoid rapid switching. Using range finder for height requires a consistent terrain height + * which cannot be assumed if the vehicle is moving horizontally. + */ + if ((aboveUpperSwHgt || dontTrustTerrain) && (activeHgtSource == HGT_SOURCE_RNG)) { + // cannot trust terrain or range finder so stop using range finder height + if (frontend->_altSource == 0) { + activeHgtSource = HGT_SOURCE_BARO; + } else if (frontend->_altSource == 2) { + activeHgtSource = HGT_SOURCE_GPS; } + } else if (belowLowerSwHgt && trustTerrain && (prevTnb.c.z >= 0.7f)) { + // reliable terrain and range finder so start using range finder height + activeHgtSource = HGT_SOURCE_RNG; } + } else if (frontend->_altSource == 0) { + activeHgtSource = HGT_SOURCE_BARO; } else if ((frontend->_altSource == 2) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) < 500) && validOrigin && gpsAccuracyGood) { activeHgtSource = HGT_SOURCE_GPS; } else if ((frontend->_altSource == 3) && validOrigin && rngBcnGoodToAlign) { activeHgtSource = HGT_SOURCE_BCN; - } else { - activeHgtSource = HGT_SOURCE_BARO; } // Use Baro alt as a fallback if we lose range finder, GPS or external nav