TECS——ArduPilot——更新速度

版权声明：本文为博主原创博文，未经允许不得转载，若要转载，请说明出处并给出博文链接 

本文对应着ArduPilot中AP_TECS库中的用来更新飞机飞行速度的源代码，分享一下自己的体会。

void AP_TECS::_update_speed(float load_factor)
{
    // 计算更新时间/采样间隔
    uint64_t now = AP_HAL::micros64();
    float DT = (now - _update_speed_last_usec) * 1.0e-6f;
    _update_speed_last_usec = now;

    // 转换等价的空速为真实的空速

    float EAS2TAS = _ahrs.get_EAS2TAS();
    _TAS_dem = _EAS_dem * EAS2TAS;
    _TASmax   = aparm.airspeed_max * EAS2TAS;
    _TASmin   = aparm.airspeed_min * EAS2TAS;

    if (aparm.stall_prevention) {
        // 当失速发生时，根据 aerodynamic load factor 生成一个最小空速
        _TASmin *= load_factor;
    }

    if (_TASmax < _TASmin) {
        _TASmax = _TASmin;
    }
    if (_TASmin > _TAS_dem) {
        _TASmin = _TAS_dem;
    }

    // 如果距离上次更新太久，就重置时间
    if (DT > 1.0f) {
        _TAS_state = (_EAS * EAS2TAS);
        _integDTAS_state = 0.0f;
        DT            = 0.1f; // 第一次开启TECS，使用一个较小的时间常量
    }

    // 获取空速，或者在没有使用空速并且设置速度的速率为0的情况下，默认空速为最大最小值和的一半
    bool use_airspeed = _use_synthetic_airspeed_once || _use_synthetic_airspeed.get() || _ahrs.airspeed_sensor_enabled();
    if (!use_airspeed || !_ahrs.airspeed_estimate(&_EAS)) {
        _EAS = 0.5f * (aparm.airspeed_min.get() + (float)aparm.airspeed_max.get());
    }

    // 执行一个二阶互补滤波来获得比较圆滑的空速估计值，并且保存到_TAS_state
    float aspdErr = (_EAS * EAS2TAS) - _TAS_state;
    float integDTAS_input = aspdErr * _spdCompFiltOmega * _spdCompFiltOmega;
    // 防止空速被卷起
    if (_TAS_state < 3.1f) {
        integDTAS_input = MAX(integDTAS_input , 0.0f);
    }
    _integDTAS_state = _integDTAS_state + integDTAS_input * DT;
    float TAS_input = _integDTAS_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f;
    _TAS_state = _TAS_state + TAS_input * DT;
    // 限制空速最小为 3 m/s
    _TAS_state = MAX(_TAS_state, 3.0f);

}

从程序中可以看到主要就是一些逻辑判断和这个二阶互补滤波（比较难理解）。

_spdComFiltOmega 是用于融合X轴（向前）加速度和空速的互补滤波器的交叉频率，为了获得较低的空速噪声和滞后估计;

integDTAS_input可以理解为空速的二次微分项，即空气加加速度;

TAS_input可以理解为空速的一次微分项，即空气加速度。

_vel_dot =重力加速度在水平X轴上的分量与加速度计x轴的数值之和。

// Update and average speed rate of change
// Get DCM
    const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned();
// Calculate speed rate of change
    float temp = rotMat.c.x * GRAVITY_MSS + _ahrs.get_ins().get_accel().x;
// take 5 point moving average
    _vel_dot = _vdot_filter.apply(temp);

load_factor = aerodynamic_load_factor，初始值为1。后续被update_flight_mode函数中的除去垂起相关模式以及手动、stabilize、Acro等其余模式下每400Hz更新一次，具体更新如下代码

    float demanded_roll = fabsf(nav_roll_cd*0.01f);
    if (demanded_roll > 85) {
        // 限制roll的目标需求最大为85度，以防临近90出现数据错误（奇异问题）
        demanded_roll = 85;
    }
    aerodynamic_load_factor = 1.0f / safe_sqrt(cosf(radians(demanded_roll)));

本文提到的二阶互补滤波类同ArduPilot-AP_TECS-高度与爬升速度的估计（3阶互补滤波），可以互相参照