博客

首先从copter.cpp中的fast_loop开始



进入到motors_output()函数中



上述函数中完成的有一些解锁检查, 进入之后如下图所示



这个函数中完成由



τ

\tau






τ





, thrust到 PWM的输出转换过程



首先在output_armed_stabilizing()函数中完成系数因子*



τ

\tau






τ





, 具体代码为

for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
    if (motor_enabled[i]) {
        // calculate the thrust outputs for roll and pitch
        _thrust_rpyt_out[i] = roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i];
        // record lowest roll + pitch command
        if (_thrust_rpyt_out[i] < rp_low) {
            rp_low = _thrust_rpyt_out[i];
        }
        // record highest roll + pitch command
        if (_thrust_rpyt_out[i] > rp_high && (!_thrust_boost || i != _motor_lost_index)) {
            rp_high = _thrust_rpyt_out[i];
        }

        // Check the maximum yaw control that can be used on this channel
        // Exclude any lost motors if thrust boost is enabled
        if (!is_zero(_yaw_factor[i]) && (!_thrust_boost || i != _motor_lost_index)){
            if (is_positive(yaw_thrust * _yaw_factor[i])) {
                yaw_allowed = MIN(yaw_allowed, fabsf(MAX(1.0f - (throttle_thrust_best_rpy + _thrust_rpyt_out[i]), 0.0f)/_yaw_factor[i]));
            } else {
                yaw_allowed = MIN(yaw_allowed, fabsf(MAX(throttle_thrust_best_rpy + _thrust_rpyt_out[i], 0.0f)/_yaw_factor[i]));
            }
        }
    }
}

关键参数为 _roll_factor[i], _pitch_factor[i], 以及后面出现的_yaw_facotr[i], 对应的thrust为

// add scaled roll, pitch, constrained yaw and throttle for each motor
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
    if (motor_enabled[i]) {
        _thrust_rpyt_out[i] = throttle_thrust_best_rpy + thr_adj + (rpy_scale * _thrust_rpyt_out[i]);
    }
}

随后在void AP_MotorsMatrix::output_to_motors()函数中, 将上述计算出来的_thrust_rpyt_out转换赋值给_actuator[i], 具体代码为



最后完成输出.

ＰＳ: 比例因子的给定在　ＡＰ_Veｈｉｃｌｅ .cpp 中的init_ardupilot()中的 ｉｎｉｔ_rc_out()函数中完成的初始化

// init
void AP_MotorsMatrix::init(motor_frame_class frame_class, motor_frame_type frame_type)
{
    // record requested frame class and type
    _last_frame_class = frame_class;
    _last_frame_type = frame_type;

    // setup the motors
    setup_motors(frame_class, frame_type);

    // enable fast channels or instant pwm
    set_update_rate(_speed_hz);
}