From e85539acf25a2d1ee69e3eac1c7d616445f6f144 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:47 +1100 Subject: [PATCH] AP_NavEKF2: allow for more than 327m range rangefinders --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 4 ++-- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index b58f2254db..be95fcbc74 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -483,7 +483,7 @@ const AP_Param::GroupInfo NavEKF2::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). This will not work unless Baro or GPS height is selected as the primary height source vis EK2_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. + // @Description: Range finder can be used as the primary height source when below this percentage of its maximum range (see RNGFND*_MAX). This will not work unless Baro or GPS height is selected as the primary height source vis EK2_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_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index cdd25bc133..724b2a2e05 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -28,7 +28,7 @@ void NavEKF2_core::readRangeFinder(void) return; } - rngOnGnd = MAX(_rng->ground_clearance_cm_orient(ROTATION_PITCH_270) * 0.01f, 0.05f); + rngOnGnd = MAX(_rng->ground_clearance_orient(ROTATION_PITCH_270), 0.05f); // read range finder at 20Hz // TODO better way of knowing if it has new data @@ -51,7 +51,7 @@ void NavEKF2_core::readRangeFinder(void) rngMeasIndex[sensorIndex] = 0; } storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25; - storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance_cm() * 0.01f; + storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance(); } else { continue; } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 6d5937dd22..7f209b6b28 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -66,7 +66,7 @@ bool NavEKF2_core::getHeightControlLimit(float &height) const // we really, really shouldn't be here. return false; } - height = MAX(float(_rng->max_distance_cm_orient(ROTATION_PITCH_270)) * 0.007f - 1.0f, 1.0f); + height = MAX(float(_rng->max_distance_orient(ROTATION_PITCH_270)) * 0.7f - 1.0f, 1.0f); #else return false; #endif diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index debcaaf2c2..2f3382a8e9 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -979,7 +979,7 @@ void NavEKF2_core::selectHeightForFusion() activeHgtSource = HGT_SOURCE_RNG; } else if ((frontend->_useRngSwHgt > 0) && ((frontend->_altSource == 0) || (frontend->_altSource == 2)) && _rng && rangeFinderDataIsFresh) { // determine if we are above or below the height switch region - ftype rangeMaxUse = 1e-4f * (float)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt; + const ftype rangeMaxUse = 1e-2f * _rng->max_distance_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt; bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse; bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse;