AC_AttitudeControl_Heli.cpp的AC_AttitudeControl_Heli::rate_target_to_motor_yaw函数代码分析

代码基于ardupilot3.4.2RC2，仅分析ArduCopter（多旋翼、直升机）构型中代码逻辑、使用关系，其他构型（飞机、云台、车）后续考虑分析。

float AC_AttitudeControl_Heli::rate_target_to_motor_yaw(float rate_target_rads)

函数作用：

速率控制器计算电机输出，控制机体偏航角速率的达到目标速率，以弧度/秒为单位。

函数全局调用情况如下：

 函数声明情况如下：

 函数声明文件如下：

 函数流程图如下：

 函数逻辑顺序图如下：

函数原始代码如下：

// rate_bf_to_motor_yaw - ask the rate controller to calculate the motor outputs to achieve the target rate in radians/second速率控制器计算电机输出，控制机体偏航角速率的达到目标速率，以弧度/秒为单位。

float AC_AttitudeControl_Heli::rate_target_to_motor_yaw(float rate_target_rads)

{

    float pd,i,vff;     // used to capture pid values for logging用于捕获用于记录的pid值

    float current_rate_rads;     // this iteration's rate这个迭代的速度

    float rate_error_rads;       // simply target_rate - current_rate目标偏航角速率与当前偏航角速率的误差

    float yaw_out;

    // get current rate获得当前速率

    // To-Do: make getting gyro rates more efficient?获得机体当前的偏航角速率

    current_rate_rads = _ahrs.get_gyro().z;

    // calculate error and call pid controller计算误差，调用PID控制器

    rate_error_rads  = rate_target_rads - current_rate_rads;

    // pass error to PID controller输入误差到PID控制器

    _pid_rate_yaw.set_input_filter_all(rate_error_rads);

    _pid_rate_yaw.set_desired_rate(rate_target_rads);

    // get p and d获得P、D

    pd = _pid_rate_yaw.get_p() + _pid_rate_yaw.get_d();

    // get i term获得积分项

    i = _pid_rate_yaw.get_integrator();

    // update i term as long as we haven't breached the limits or the I term will certainly reduce更新I项，要求不超限，或正在减少

    if (!_flags_heli.limit_yaw || ((i>0&&rate_error_rads<0)||(i<0&&rate_error_rads>0))) {

        if (((AP_MotorsHeli&)_motors).rotor_runup_complete()) {

            i = _pid_rate_yaw.get_i();

        } else {

            i = ((AC_HELI_PID&)_pid_rate_yaw).get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);    // If motor is not running use leaky I-term to avoid excessive build-up如果电机未运行，则使用泄漏的I项，以避免过度积聚

        }

    }

        // For legacy reasons, we convert to centi-degrees before inputting to the feedforward换为度的单位，并输入到反馈

    vff = yaw_velocity_feedforward_filter.apply(_pid_rate_yaw.get_vff(rate_target_rads), _dt);

   

    // add feed forward加反馈并输出

    yaw_out = pd + i + vff;

    // constrain output and update limit flag限制输出并更新限制标志

    if (fabsf(yaw_out) > AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX) {

        yaw_out = constrain_float(yaw_out,-AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX,AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX);

        _flags_heli.limit_yaw = true;

    }else{

        _flags_heli.limit_yaw = false;

    }

    // output to motors输出电机控制

    return yaw_out;

}