TFmini定高雷达和MB12XX声纳都开启的话，Range sensor会启用哪个

wilsonleong

声纳在PX4FLOW上，SENS_EN_PX4FLOW，SENS_EN_MB12XX，SENS_TFMINI_CFG都启用了，EKF2_RNG_AID也启用，EKF2_HGT_MODE选择的是Vision。
TFmini定高雷达或者MB12XX声纳单独启用时候都可以正常读到数据，
但是TFmini定高雷达和MB12XX声纳同时启用的时候，究竟用的是哪一个呢？怎么看实际用的是哪一个？可以配置么？
因为我还要配置EKF2_RNG_POS_*参数，同时启用的时候不知道应该按照哪个传感器来配置。

我知道答案 回答被采纳将会获得10 阿木币 已有6人回答

blueboats

这个个人感觉要看一下定高那部分数据融合的代码吧，EKF那个课程里面好像有讲过。高度数据的话，有一个准确的就行吧，RTK挺准的，单TFmini飞无人机会明显的掉高么。

eason

飞控融合后的高度数据 Alt(Rel)；tfmini的本身数据 Down。

从你的参数配置来看，ekf2_ght_mode选择了vision，也就是选择光流作为高度数据的主要来源，tfmini的数据也会参数ekf的融合。你可以这样做，先将ght数据来源选择range sensor，然后对比融合后的高度数据和tfmini本身的数据，是不是融合的很准；同样也试试hgt选择vision后看看实际融合后高度准不准，哪一个准确就用哪一个？然后配置相应的pos_x位置相对坐标

wilsonleong

如果参考ROS的消息通信进行类比，TFmini定高雷达和MB12XX声纳会抢着向distance_sensor这个uORB消息发布数据，不知道对不对?

wilsonleong

如果参考ROS的消息通信进行类比，TFmini定高雷达和MB12XX声纳会抢着向distance_sensor这个uORB消息发布数据，不知道对不对?

amov_msq

wilsonleong 发表于 2020-8-10 11:59
如果参考ROS的消息通信进行类比，TFmini定高雷达和MB12XX声纳会抢着向distance_sensor这个uORB消息发布数据 ...

如果发送消息类型一样是肯定的，但是一般没人这样做啊，硬件接口本来就不是很多，有些硬件接口都是复用的。

wilsonleong

EKF2_RNG_AID只有选择气压计或者GPS作为高度源才有效

checkRangeAidSuitability();
const bool do_range_aid = (_params.range_aid == 1) && isRangeAidSuitable();

bool fuse_height = false;

switch (_params.vdist_sensor_type) {
default:
    ECL_ERR("Invalid hgt mode: %d", _params.vdist_sensor_type);

// FALLTHROUGH
case VDIST_SENSOR_BARO:
    if (do_range_aid && _range_sensor.isDataHealthy()) {
        setControlRangeHeight();
        fuse_height = true;

        // we have just switched to using range finder, calculate height sensor offset such that current
        // measurement matches our current height estimate
        if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
            _hgt_sensor_offset = _terrain_vpos;
        }

    } else if (!do_range_aid && _baro_data_ready && !_baro_hgt_faulty) {
        startBaroHgtFusion();
        fuse_height = true;

    } else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_intermittent) {
        // switch to gps if there was a reset to gps
        fuse_height = true;

        // we have just switched to using gps height, calculate height sensor offset such that current
        // measurement matches our current height estimate
        if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) {
            _hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
        }
    }

    break;

case VDIST_SENSOR_RANGE:
    if (_range_sensor.isDataHealthy()) {
        setControlRangeHeight();
        fuse_height = true;

        if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
            // we have just switched to using range finder, calculate height sensor offset such that current
            // measurement matches our current height estimate
            // use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
            if (_control_status.flags.in_air && isTerrainEstimateValid()) {
                _hgt_sensor_offset = _terrain_vpos;

            } else if (_control_status.flags.in_air) {
                _hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2);

            } else {
                _hgt_sensor_offset = _params.rng_gnd_clearance;
            }
        }

    } else if (_baro_data_ready && !_baro_hgt_faulty) {
        startBaroHgtFusion();
        fuse_height = true;
    }

    break;

case VDIST_SENSOR_GPS:

    // NOTE: emergency fallback due to extended loss of currently selected sensor data or failure
    // to pass innovation cinsistency checks is handled elsewhere in Ekf::controlHeightSensorTimeouts.
    // Do switching between GPS and rangefinder if using range finder as a height source when close
    // to ground and moving slowly. Also handle switch back from emergency Baro sensor when GPS recovers.
    if (!_control_status_prev.flags.rng_hgt && do_range_aid && _range_sensor.isDataHealthy()) {
        setControlRangeHeight();

        // we have just switched to using range finder, calculate height sensor offset such that current
        // measurement matches our current height estimate
        _hgt_sensor_offset = _terrain_vpos;

    } else if (_control_status_prev.flags.rng_hgt && !do_range_aid) {
        // must stop using range finder so find another sensor now
        if (!_gps_hgt_intermittent && _gps_checks_passed) {
            // GPS quality OK
            startGpsHgtFusion();
        } else if (!_baro_hgt_faulty) {
            // Use baro as a fallback
            startBaroHgtFusion();
        }
    } else if (_control_status.flags.baro_hgt && !do_range_aid && !_gps_hgt_intermittent && _gps_checks_passed) {
        // In baro fallback mode and GPS has recovered so start using it
        startGpsHgtFusion();
    }
    if (_control_status.flags.gps_hgt && _gps_data_ready) {
        fuse_height = true;
    } else if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
        fuse_height = true;
    } else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
        fuse_height = true;
    }
    break;

case VDIST_SENSOR_EV:

    // don't start using EV data unless data is arriving frequently
    if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
        fuse_height = true;
        setControlEVHeight();
        resetHeight();
    }

    if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
        // switch to baro if there was a reset to baro
        fuse_height = true;
    }

    // determine if we should use the vertical position observation
    if (_control_status.flags.ev_hgt) {
        fuse_height = true;
    }

    break;
}