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階互補濾波），可以互相參照