关于TFminiPlus定高雷达，是不是直接能得到离地垂直高度？

wilsonleong

关于TFminiPlus定高雷达，是不是直接能得到离地垂直高度？机体倾斜需不需要自己去解算实际垂直高度？QGC地面站上要做相关设置么？

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

AmovlabQYP

TFminiPlus定高雷达就是一个距离传感器，只输出点对点的距离。

飞机倾斜时，要想知道垂直高度，你要自己根据角度解算，你想这么做的话，要自己改一下源码。

但是觉得没必要，正常时候都不会有那么多倾斜和偏差

wilsonleong

直接从distance_sensor消息中拿到的数据是原始数据，不带倾斜修正的；在EKF2里面会修正成垂直地面的高度使用。 control.cpp

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;

sensor_range_finder.hpp

float getDistBottom() const { return _sample.rng * _cos_tilt_rng_to_earth; }

sensor_range_finder.cpp

void SensorRangeFinder::updateSensorToEarthRotation(const Dcmf &R_to_earth)
{
    // calculate 2,2 element of rotation matrix from sensor frame to earth frame
    // this is required for use of range finder and flow data
    _cos_tilt_rng_to_earth = R_to_earth(2, 0) * _sin_pitch_offset + R_to_earth(2, 2) * _cos_pitch_offset;
}

风帆

wilsonleong 发表于 2020-8-10 19:06
[md]直接从[distance_sensor](https://github.com/PX4/Firmware/blob/master/msg/distance_sensor.msg)消 ...

这种从源码解析功能的习惯真的特别好，感谢感谢，让我们也学习了。。。