EKF2.cpp: Rangefinder instance selection not stable

[ecmnet]

Having two rangefinders on board (e.g. with HereFlow and a Lightware sensor), the selection of the rangefinder instance depends on the sequence of published UORB messages: https://github.com/PX4/PX4-Autopilot/blob/f65781025ebb776fb5c406d96fbbfa52360f8a30/src/modules/ekf2/EKF2.cpp#L1557 I noticed, that with UAVCAN enabled for HereFlow, sometimes, the distance sensor of the flow module is selected in EKF2 instead of the Lightware sensor. Due to the max_range, the lightwave sensor should be preferred. My proposal would be to select the rangefinder, that has the highest maximum range instead of the first one found. This search might be limited to a certain start-up time or should be - as a fallback - continuously checked.

Release: 1.12 beta1

[dagar]

Yes, we need to do something to handle them more explicitly and likely monitor all. The logic exists primarily to filter out non-downward facing options, but then I added a simple timeout to possibly benefit if there's a failure and you have multiple usable options.

[ecmnet]

@dagar Some questions:

• What do you think about the idea using the instance that has the highest max_distance value?
• I saw that the instance determination is performed only once. Any disadvantages, if the check would be determined continuously?

I would propose to solve it like this: `

 // get subscription index of first downward-facing range sensor
uORB::SubscriptionMultiArray<distance_sensor_s> distance_sensor_subs{ORB_ID::distance_sensor};

// Search for the rangefinder with highest max_distance
    int8_t selected_instance = -1;
    for (int8_t i = 0; i < distance_sensor_subs.size(); i++) {
        distance_sensor_s distance_sensor;

        if (distance_sensor_subs[i].copy(&distance_sensor)) {
            // use the instance with highest max distance property
            if ((hrt_elapsed_time(&distance_sensor.timestamp) < 100_ms)
                    && (distance_sensor.orientation
                            == distance_sensor_s::ROTATION_DOWNWARD_FACING)
                    && (distance_sensor.max_distance
                            > _distance_max_distance)) {
                _distance_max_distance = distance_sensor.max_distance;
                selected_instance = i;
            }
        }
    }

    if (selected_instance >=0 && selected_instance != _distance_sensor_selected_instance
            && _distance_sensor_sub.ChangeInstance(selected_instance)) {
        _distance_sensor_selected_instance = selected_instance;
        PX4_INFO("%d - selected distance_sensor: %d with max_distance %dm",
                _instance, selected_instance, _distance_max_distance);
    }

...

if (hrt_elapsed_time(&_last_range_sensor_update) > 1_s) {
    _distance_sensor_selected_instance = -1;
        _distance_max_distance = 0;
}`

This continuously searches for the instance with the highest max_distance and falls back if not available anymore.

The search could be limited to cases where

• the selected range finder has a timeout
• the number of range finder instances changes