Ardupilot: EKF is not using RangeFinder when EK2_RNG_USE_HGT > 0, EK2_ALT_SOURCE = 0

[fs007]

Copter 4.0 (actually this bug existed since copter 3.4)

There is a coding error in AP_NavEKF2_PosVelFusion.cpp that prevents the rangefinder from being used as primary Alt-Source even if the EK2 parameters are set accordingly (EK2_RNG_USE_HGT > 0; EK2_ALT_SOURCE = 0)

No matter, which value frontend ->_altSource has, all the code of “// determine if we are above or below the height switch region” is NEVER executed !

Discussion here

// select height source
if (extNavUsedForPos) {
    // always use external vision as the height source if using for position.
    activeHgtSource = HGT_SOURCE_EV;
} else if (((frontend->_useRngSwHgt > 0) && (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) {
    if (frontend->_altSource == 1) {
        // always use range finder
        activeHgtSource = HGT_SOURCE_RNG;
    } else {
        // determine if we are above or below the height switch region
        float rangeMaxUse = 1e-4f * (float)frontend->_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
        float horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y);
        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
         * 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;
        }
    }
} 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;
}

[anbello]

@rmackay9 I see that in the "Copter: 4.0 issues list" https://github.com/ArduPilot/ardupilot/issues/12547 this issue is under "Could not be verified OR not a problem" but i think that something should be done for this reason:

• as already pointed there is dead code after PR https://github.com/ArduPilot/ardupilot/pull/12250
• the param EK2_RNG_USE_HGT is almost useless because only with EK2_RNG_USE_HGT > 0 AND EK2_ALT_SOURCE = 1 the rangefinder becomes primary alt source. It could be useful if things worked as stated by @priseborough here https://github.com/ArduPilot/ardupilot/pull/12250#issuecomment-533708253
• with rangefinder as primary alt source if the copter goes over the limit set by RNGFND_MAX_CM there is no switch to baro and the copter continue to climb even with throttle at middle (tested both on SITL and on real flight).