Ardupilot动力分配-混控部分分析

77 阅读 0 评论 51 点赞

我是靠谱客的博主炙热苗条，最近开发中收集的这篇文章主要介绍Ardupilot动力分配-混控部分分析，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

首先从copter.cpp中的fast_loop开始
在这里插入图片描述
进入到motors_output()函数中

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

这个函数中完成由 $τ$ , thrust到 PWM的输出转换过程

首先在output_armed_stabilizing()函数中完成系数因子* $τ$ , 具体代码为

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);
}