AC_AttitudeControl_Heli.cpp的void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch函数代码分析

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

void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target_rads, float rate_pitch_target_rads)

函数作用：。

机体俯仰、倾斜角速率控制的执行，由速率控制器计算电机输出，以达到以弧度/秒为单位的目标速率

main函数全局调用情况如下：五个地方出现

 

函数声明情况如下：

 

函数声明文件如下：

 

函数流程图如下：

 

 

函数逻辑顺序图如下：

 

函数原始代码如下：

//

// body-frame rate controller机体角速率控制器

//

// rate_bf_to_motor_roll_pitch - ask the rate controller to calculate the motor outputs to achieve the target rate in radians/second

void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target_rads, float rate_pitch_target_rads)

{

    float roll_pd, roll_i, roll_ff;             // used to capture pid values滚转的PID数值

    float pitch_pd, pitch_i, pitch_ff;          // used to capture pid values俯仰的PID数值

    float rate_roll_error_rads, rate_pitch_error_rads;    // simply target_rate - current_rate当前俯仰和滚转的误差

    float roll_out, pitch_out;

    const Vector3f& gyro = _ahrs.get_gyro();     // get current rates得到角速率

    // calculate error计算误差

    rate_roll_error_rads = rate_roll_target_rads - gyro.x;

    rate_pitch_error_rads = rate_pitch_target_rads - gyro.y;

    // pass error to PID controller传递误差给PID控制器

    _pid_rate_roll.set_input_filter_all(rate_roll_error_rads);

    _pid_rate_roll.set_desired_rate(rate_roll_target_rads);

    _pid_rate_pitch.set_input_filter_all(rate_pitch_error_rads);

    _pid_rate_pitch.set_desired_rate(rate_pitch_target_rads);

    // call p and d controllers调用P和D控制器

    roll_pd = _pid_rate_roll.get_p() + _pid_rate_roll.get_d();

    pitch_pd = _pid_rate_pitch.get_p() + _pid_rate_pitch.get_d();

    // get roll i term获得滚转的积分

    roll_i = _pid_rate_roll.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_roll || ((roll_i>0&&rate_roll_error_rads<0)||(roll_i<0&&rate_roll_error_rads>0))){

        if (_flags_heli.leaky_i){

            roll_i = _pid_rate_roll.get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);

        }else{

            roll_i = _pid_rate_roll.get_i();

        }

    }

    // get pitch i term获得俯仰的积分

    pitch_i = _pid_rate_pitch.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_pitch || ((pitch_i>0&&rate_pitch_error_rads<0)||(pitch_i<0&&rate_pitch_error_rads>0))){

        if (_flags_heli.leaky_i) {

            pitch_i = _pid_rate_pitch.get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);

        }else{

            pitch_i = _pid_rate_pitch.get_i();

        }

    }

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

    roll_ff = roll_feedforward_filter.apply(_pid_rate_roll.get_vff(rate_roll_target_rads), _dt);

    pitch_ff = pitch_feedforward_filter.apply(_pid_rate_pitch.get_vff(rate_pitch_target_rads), _dt);

    // add feed forward and final output加反馈并输出

    roll_out = roll_pd + roll_i + roll_ff;

    pitch_out = pitch_pd + pitch_i + pitch_ff;

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

    if (fabsf(roll_out) > AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX) {

        roll_out = constrain_float(roll_out,-AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX,AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX);

        _flags_heli.limit_roll = true;

    }else{

        _flags_heli.limit_roll = false;

    }

    if (fabsf(pitch_out) > AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX) {

        pitch_out = constrain_float(pitch_out,-AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX,AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX);

        _flags_heli.limit_pitch = true;

    }else{

        _flags_heli.limit_pitch = false;

    }

    // output to motors输出电机控制

    _motors.set_roll(roll_out);

    _motors.set_pitch(pitch_out);

    // Piro-Comp, or Pirouette Compensation is a pre-compensation calculation, which basically rotates the Roll and Pitch Rate I-terms as the

    // helicopter rotates in yaw.  Much of the built-up I-term is needed to tip the disk into the incoming wind.  Fast yawing can create an instability

    // as the built-up I-term in one axis must be reduced, while the other increases.  This helps solve that by rotating the I-terms before the error occurs.

    // It does assume that the rotor aerodynamics and mechanics are essentially symmetrical about the main shaft, which is a generally valid assumption.

Piro Comp或Pirouette补偿是一种预补偿计算，基本上随着直升机在偏航中旋转，旋转横滚和俯仰率I项。大部分累积的I项需要将圆盘倾斜到迎面而来的风中。快速偏航会造成不稳定性，因为一个轴上的累积I项必须减少，而另一个轴上的累积I项则增加。这有助于在错误发生之前旋转I项来解决这个问题。它确实假设转子的空气动力学和力学基本上与主轴对称，这是一个普遍有效的假设。

    if (_piro_comp_enabled){

        int32_t         piro_roll_i, piro_pitch_i;            // used to hold I-terms while doing piro comp

        piro_roll_i  = roll_i;

        piro_pitch_i = pitch_i;

        Vector2f yawratevector;

        yawratevector.x     = cosf(-_ahrs.get_gyro().z * _dt);

        yawratevector.y     = sinf(-_ahrs.get_gyro().z * _dt);

        yawratevector.normalize();

        roll_i      = piro_roll_i * yawratevector.x - piro_pitch_i * yawratevector.y;

        pitch_i     = piro_pitch_i * yawratevector.x + piro_roll_i * yawratevector.y;

        _pid_rate_pitch.set_integrator(pitch_i);

        _pid_rate_roll.set_integrator(roll_i);

    }

}