Ardupilot---AP_TECS 框架理解

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

本文针对AP_TECS的框架给出对应代码及个人理解体会。

首先进行update_50hz（）这个函数，50hz更新一次，该函数会更新飞机的高度h、向上爬升速率climb_rate。

接下来，就是TECS的核心工作流程，即下面这段代码（10hz更新一次）。其中会有一些控制逻辑诸如限幅、滤波等不做详解。

void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
                                    int32_t EAS_dem_cm,
                                    enum AP_Vehicle::FixedWing::FlightStage flight_stage,
                                    float distance_beyond_land_wp,
                                    int32_t ptchMinCO_cd,
                                    int16_t throttle_nudge,
                                    float hgt_afe,
                                    float load_factor,
                                    bool soaring_active)
{
    // Calculate time in seconds since last update
    uint64_t now = AP_HAL::micros64();
    _DT = (now - _update_pitch_throttle_last_usec) * 1.0e-6f;
    _update_pitch_throttle_last_usec = now;

    _flags.is_doing_auto_land = (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND);
    _distance_beyond_land_wp = distance_beyond_land_wp;
    _flight_stage = flight_stage;

    // Convert inputs
    _hgt_dem = hgt_dem_cm * 0.01f;
    _EAS_dem = EAS_dem_cm * 0.01f;

    // 二阶互补滤波器估计出速度 
    // （参见博客https://blog.csdn.net/u012814946/article/details/81290163）
    _update_speed(load_factor);

    if (aparm.takeoff_throttle_max != 0 &&
            (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND)) {
        _THRmaxf  = aparm.takeoff_throttle_max * 0.01f;
    } else {
        _THRmaxf  = aparm.throttle_max * 0.01f;
    }
    _THRminf  = aparm.throttle_min * 0.01f;

    // work out the maximum and minimum pitch
    // if TECS_PITCH_{MAX,MIN} isn't set then use
    // LIM_PITCH_{MAX,MIN}. Don't allow TECS_PITCH_{MAX,MIN} to be
    // larger than LIM_PITCH_{MAX,MIN}
    if (_pitch_max <= 0) {
        _PITCHmaxf = aparm.pitch_limit_max_cd * 0.01f;
    } else {
        _PITCHmaxf = MIN(_pitch_max, aparm.pitch_limit_max_cd * 0.01f);
    }

    if (_pitch_min >= 0) {
        _PITCHminf = aparm.pitch_limit_min_cd * 0.01f;
    } else {
        _PITCHminf = MAX(_pitch_min, aparm.pitch_limit_min_cd * 0.01f);
    }

    // apply temporary pitch limit and clear
    if (_pitch_max_limit < 90) {
        _PITCHmaxf = constrain_float(_PITCHmaxf, -90, _pitch_max_limit);
        _PITCHminf = constrain_float(_PITCHminf, -_pitch_max_limit, _PITCHmaxf);
        _pitch_max_limit = 90;
    }
        
    if (_landing.is_flaring()) {
        // in flare use min pitch from LAND_PITCH_CD
        _PITCHminf = MAX(_PITCHminf, _landing.get_pitch_cd() * 0.01f);

        // and use max pitch from TECS_LAND_PMAX
        if (_land_pitch_max != 0) {
            _PITCHmaxf = MIN(_PITCHmaxf, _land_pitch_max);
        }

        // and allow zero throttle
        _THRminf = 0;
    } else if (_landing.is_on_approach() && (-_climb_rate) > _land_sink) {
        // constrain the pitch in landing as we get close to the flare
        // point. Use a simple linear limit from 15 meters after the
        // landing point
        float time_to_flare = (- hgt_afe / _climb_rate) - _landing.get_flare_sec();
        if (time_to_flare < 0) {
            // we should be flaring already
            _PITCHminf = MAX(_PITCHminf, _landing.get_pitch_cd() * 0.01f);
        } else if (time_to_flare < timeConstant()*2) {
            // smoothly move the min pitch to the flare min pitch over
            // twice the time constant
            float p = time_to_flare/(2*timeConstant());
            float pitch_limit_cd = p*aparm.pitch_limit_min_cd + (1-p)*_landing.get_pitch_cd();
#if 0
            ::printf("ttf=%.1f hgt_afe=%.1f _PITCHminf=%.1f pitch_limit=%.1f climb=%.1f\n",
                     time_to_flare, hgt_afe, _PITCHminf, pitch_limit_cd*0.01f, _climb_rate);
#endif
            _PITCHminf = MAX(_PITCHminf, pitch_limit_cd*0.01f);
        }
    }

    if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
        if (!_flags.reached_speed_takeoff && _TAS_state >= _TAS_dem_adj) {
            // we have reached our target speed in takeoff, allow for
            // normal throttle control
            _flags.reached_speed_takeoff = true;
        }
    }
    
    // convert to radians
    _PITCHmaxf = radians(_PITCHmaxf);
    _PITCHminf = radians(_PITCHminf);

    // initialise selected states and variables if DT > 1 second or in climbout
    _initialise_states(ptchMinCO_cd, hgt_afe);

    // 更新总体能量速率限制
    _update_STE_rate_lim();

    // 更新出速度的目标值
    _update_speed_demand();

    // 更新出高度的目标值
    _update_height_demand();

    // 检测是否欠速
    _detect_underspeed();

    // 更新势能、动能的目标与估计值
    _update_energies();

    // 更新油门的目标值 - 如果没有空速计，就用简单的pitch到油门
    // Note that caller can demand the use of
    // synthetic airspeed for one loop if needed. This is required
    // during QuadPlane transition when pitch is constrained
    if (_ahrs.airspeed_sensor_enabled() || _use_synthetic_airspeed || _use_synthetic_airspeed_once) {
        _update_throttle_with_airspeed();
        _use_synthetic_airspeed_once = false;
    } else {
        _update_throttle_without_airspeed(throttle_nudge);
    }

    // Detect bad descent due to demanded airspeed being too high
    _detect_bad_descent();

    // 一飞冲天时，不会触发差的下降
    if (soaring_active) {
        _flags.badDescent = false;        
    }

    // 更新出俯仰角的目标值
    _update_pitch();

    // 记下LOG
    DataFlash_Class::instance()->Log_Write(
        "TECS",
        "TimeUS,h,dh,hdem,dhdem,spdem,sp,dsp,ith,iph,th,ph,dspdem,w,f",
        "smnmnnnn----o--",
        "F0000000----0--",
        "QfffffffffffffB",
        now,
        (double)_height,
        (double)_climb_rate,
        (double)_hgt_dem_adj,
        (double)_hgt_rate_dem,
        (double)_TAS_dem_adj,
        (double)_TAS_state,
        (double)_vel_dot,
        (double)_integTHR_state,
        (double)_integSEB_state,
        (double)_throttle_dem,
        (double)_pitch_dem,
        (double)_TAS_rate_dem,
        (double)logging.SKE_weighting,
        _flags_byte);
    DataFlash_Class::instance()->Log_Write("TEC2", "TimeUS,KErr,PErr,EDelta,LF", "Qffff",
                                           now,
                                           (double)logging.SKE_error,
                                           (double)logging.SPE_error,
                                           (double)logging.SEB_delta,
                                           (double)load_factor);
}

看到这里，我们可以回顾一下《TECS的基础与理论》。

结合理论知识，我们可以对应代码找到几个关键的函数：

①_update_speed(load_factor)

     此函数主要目的是计算出真实的空速数值，详细可以参见ArduPilot—AP_TECS—更新速度

②_update_speed_demand();   

      此函数主要目的是计算出下一步需要用到的空速目标值以及空速速率（微分项）目标值，这里用到了真实空速和总能量速率的最大最小值来对空速目标值进行一些速率限制，以防超过物理性能上的限制。

③_update_height_demand();

      此函数主要目的是计算出下一步需要用到的飞机高度目标值以及高度速率（微分项）目标值

④_update_energies();

      此函数主要目的是计算出下一步需要用到的总能量，更新势能、动能以及势能微分和动能微分的目标值和实际估计值

⑤_update_throttle_with_airspeed();

      此函数主要目的是计算出下一步需要用到的油门目标值（使用空速计的情况，这里暂且不讨论不使用空速计的情况）

      采用总能量的误差作为控制器的输入，通过PID+前馈控制器，得到油门的目标值。

      油门的目标值将直接作为飞机油门的通道的输出，提高飞机螺旋桨旋转，进一步提供飞机的推力。

⑥_update_pitch();

      此函数主要目的是计算出下一步需要用到的pitch角的目标值

      这里计算了势能和动能的均衡值，即对动能和势能均进行加权，两个权重之和为2，如果动能的权重为2，那么势能的权重为0，此时飞机更加偏向动能，也就更加逼近速度目标。反之，飞机更加倾向势能，也就更加逼近飞机的高度目标，如果权重为1，说明双方兼顾，能量倾向更均衡。

// Calculate Specific Energy Balance demand, and error
    float SEB_dem      = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting;
    float SEBdot_dem   = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting;
    float SEB_error    = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
    float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);

        进一步通过均衡能量的目标值与估计值做差，计算均衡能量误差及其微分值。

        最终的pitch的目标值也是利用均衡能量误差作为控制输入，经过PID控制器输出的。

        更新得到的pitch目标值就会进入pitch controller里面，与导航观测到的pitch估计值做差，进一步做姿态角的控制，进而改变升降舵的偏移量，从而控制飞机的势能与动能的均衡。

      经过上面的一些TECS的基础理论的理解，以及相关控制流程的分析，相信大家多少会对TECS有个整体的认识了。已上全部是个人的一些学习体会，如有错误不足，还请指出，必定改正，感谢。