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

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

函数作用：传统直升机（单旋翼带尾桨）带平衡杠的特技模式，通过飞手的滚转和俯仰输入直接驱动倾斜盘（自动倾斜器）。

函数调用情况如下（项目中在哪里出现）：五个地方

 

函数声明情况如下：

 

函数声明文件如下：

 

函数流程图如下：

 

函数逻辑顺序图如下：

 

函数原始代码如下：

// passthrough_bf_roll_pitch_rate_yaw - passthrough the pilots roll and pitch inputs directly to swashplate for flybar acro mode通过飞手的滚转和俯仰输入直接驱动倾斜盘（自动倾斜器），带平衡杠的特技模式

void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf_cds)

{

    // convert from centidegrees on public interface to radians从度转换为弧度

    float yaw_rate_bf_rads = radians(yaw_rate_bf_cds*0.01f);

    // store roll, pitch and passthroughs保存滚转、俯仰、偏航

    // NOTE: this abuses yaw_rate_bf_rads

    _passthrough_roll = roll_passthrough;

    _passthrough_pitch = pitch_passthrough;

    _passthrough_yaw = degrees(yaw_rate_bf_rads)*100.0f;

    // set rate controller to use pass through设置速率控制

    _flags_heli.flybar_passthrough = true;

    // set bf rate targets to current body frame rates (i.e. relax and be ready for vehicle to switch out of acro)

    _attitude_target_ang_vel.x = _ahrs.get_gyro().x;

    _attitude_target_ang_vel.y = _ahrs.get_gyro().y;

    // accel limit desired yaw rate加速度限制

    if (get_accel_yaw_max_radss() > 0.0f) {

        float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;

        float rate_change_rads = yaw_rate_bf_rads - _attitude_target_ang_vel.z;

        rate_change_rads = constrain_float(rate_change_rads, -rate_change_limit_rads, rate_change_limit_rads);

        _attitude_target_ang_vel.z += rate_change_rads;

    } else {

        _attitude_target_ang_vel.z = yaw_rate_bf_rads;

    }

    integrate_bf_rate_error_to_angle_errors();

    _att_error_rot_vec_rad.x = 0;

    _att_error_rot_vec_rad.y = 0;

    // update our earth-frame angle targets更新角速率

    Vector3f att_error_euler_rad;

    // convert angle error rotation vector into 321-intrinsic euler angle difference

    // NOTE: this results an an approximation linearized about the vehicle's attitude

    if (ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _att_error_rot_vec_rad, att_error_euler_rad)) {

        _attitude_target_euler_angle.x = wrap_PI(att_error_euler_rad.x + _ahrs.roll);

        _attitude_target_euler_angle.y = wrap_PI(att_error_euler_rad.y + _ahrs.pitch);

        _attitude_target_euler_angle.z = wrap_2PI(att_error_euler_rad.z + _ahrs.yaw);

    }

    // handle flipping over pitch axis处理俯仰的跳动

    if (_attitude_target_euler_angle.y > M_PI/2.0f) {

        _attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + M_PI);

        _attitude_target_euler_angle.y = wrap_PI(M_PI - _attitude_target_euler_angle.x);

        _attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + M_PI);

    }

    if (_attitude_target_euler_angle.y < -M_PI/2.0f) {

        _attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + M_PI);

        _attitude_target_euler_angle.y = wrap_PI(-M_PI - _attitude_target_euler_angle.x);

        _attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + M_PI);

    }

    // convert body-frame angle errors to body-frame rate targets

    _rate_target_ang_vel = update_ang_vel_target_from_att_error(_att_error_rot_vec_rad);

    // set body-frame roll/pitch rate target to current desired rates which are the vehicle's actual rates期望的俯仰、倾斜

    _rate_target_ang_vel.x = _attitude_target_ang_vel.x;

    _rate_target_ang_vel.y = _attitude_target_ang_vel.y;

    // add desired target to yaw期望的偏航

    _rate_target_ang_vel.z += _attitude_target_ang_vel.z;

    _thrust_error_angle = norm(_att_error_rot_vec_rad.x, _att_error_rot_vec_rad.y);

}