目录

文章目录

• 目录
• 摘要
• 1.核心代码output_armed_stabilizing

摘要

本节主要学习ardupilot的函数：output_armed_stabilizing功能。

1.核心代码output_armed_stabilizing

void AP_MotorsMatrix::output_armed_stabilizing()
{
	//通用计数变量
    uint8_t i;
    //横滚推力输入值 范围是【-1 ,+1】
    float   roll_thrust;
    //俯仰推力输入值 范围是【-1 ,+1】
    float   pitch_thrust;
    //偏航推力输入值 范围是【-1 ,+1】
    float   yaw_thrust;
    //油门推力输入值 范围是【0 ,+1】
    float   throttle_thrust;
    //油门推力最大平均值0.0 - 1.0
    float   throttle_avg_max;
    //油门提供最大的横滚，俯仰，偏航范围不包含爬升力
    float   throttle_thrust_best_rpy;
    //用于缩放横滚、俯仰和偏航，以符合电机限制
    float   rpy_scale = 1.0f;
    //最低电机值
    float   rpy_low = 0.0f;
    //最高电机值
    float   rpy_high = 0.0f;
    //我们能适应的偏航量
    float   yaw_allowed = 1.0f;
    //我们能适应当前通道的偏航量
    float   unused_range;
    //飞行员所需油门和油门推力之间的差值=pilot's desired throttle -throttle_thrust_best_rpy
    float   thr_adj;

    //申请电压和气压补偿
    const float compensation_gain = get_compensation_gain();
    //计算横滚推力=_roll_in*系数
    roll_thrust = _roll_in * compensation_gain;
    //计算俯仰推力=_pitch_in*系数
    pitch_thrust = _pitch_in * compensation_gain;
    //计算偏航推力=_yaw_in*系数
    yaw_thrust = _yaw_in * compensation_gain;
    //获取油门推力值
    throttle_thrust = get_throttle() * compensation_gain;
    //获取油门最大推力值
    throttle_avg_max = _throttle_avg_max * compensation_gain;

    // sanity check throttle is above zero and below current limited throttle
    //正常检查油门值是高于零，低于当前油门限制值
    if (throttle_thrust <= 0.0f) 
    {
        throttle_thrust = 0.0f;
        limit.throttle_lower = true;
    }
    //限制最大油门值
    if (throttle_thrust >= _throttle_thrust_max) 
    {
        throttle_thrust = _throttle_thrust_max;
        limit.throttle_upper = true;
    }
    //油门最大平均值限制在一定范围
    throttle_avg_max = constrain_float(throttle_avg_max, throttle_thrust, _throttle_thrust_max);

    // calculate throttle that gives most possible room for yaw which is the lower of:
    //      1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible margin above the highest motor and below the lowest
    //      2. the higher of:
    //            a) the pilot's throttle input
    //            b) the point _throttle_rpy_mix between the pilot's input throttle and hover-throttle
    //      Situation #2 ensure we never increase the throttle above hover throttle unless the pilot has commanded this.
    //      Situation #2b allows us to raise the throttle above what the pilot commanded but not so far that it would actually cause the copter to rise.
    //      We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favor reducing throttle *because* it provides better yaw control)
    //      We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low.  We favor reducing throttle instead of better yaw control because the pilot has commanded it

    // calculate amount of yaw we can fit into the throttle range
    // this is always equal to or less than the requested yaw from the pilot or rate controller
    //计算为偏航提供最大可能空间的油门值，取下列值中的较小值：
    //    1. 0.5f-（rpy_low+rpy_high）/2.0-这将提供最高电机上方和最低电机下方的最大可能裕度
    //    2.较高值：
    //            a）飞行员油门输入
    //            b）_throttle_rpy_mix:飞行员输入油门和悬停油门之间
    //    情况#2确保我们绝不会将油门增加到悬停油门以上，除非飞行员已经命令这样做。
    //    情况2b允许我们把油门提高到飞行员指令的高度，但不会使直升机上升。
    //    如果这意味着减少发动机的油门，我们将选择#1（偏航控制的最佳油门）（即，我们倾向于减少油门*，因为*它提供更好的偏航控制）
    //    只有当油门很低时，我们才会选择#2（飞行员和悬停油门的组合）。我们赞成减少油门，而不是更好的偏航控制，因为飞行员已经下令了
    //计算出我们能适应油门范围的偏航量
    //这始终等于或小于飞行员或速率控制器请求的偏航
    throttle_thrust_best_rpy = MIN(0.5f, throttle_avg_max);

    // calculate roll and pitch for each motor
    // calculate the amount of yaw input that each motor can accept
    //为每个电机计算横滚和俯仰值
    //计算每个电机可以接受的偏航输入量
    for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) //最多支持12轴电机
    {
        if (motor_enabled[i]) //电机是否使能
        {
        	//计算推力=横滚推力+俯仰推力
            _thrust_rpyt_out[i] = roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i];
            //判断偏航因素是否为0 
            if (!is_zero(_yaw_factor[i]))
            {
            	//偏航因素影响是否大于0
                if (yaw_thrust * _yaw_factor[i] > 0.0f) 
                {
                	//我们能适应当前通道的偏航量
                    unused_range = fabsf(MAX(1.0f - (throttle_thrust_best_rpy + _thrust_rpyt_out[i]), 0.0f)/_yaw_factor[i]);
                    if (yaw_allowed > unused_range) 
                    {
                        yaw_allowed = unused_range;
                    }
                } else 
                {
                	//我们能适应当前通道的偏航量
                    unused_range = fabsf(MAX(throttle_thrust_best_rpy + _thrust_rpyt_out[i], 0.0f)/_yaw_factor[i]);
                    if (yaw_allowed > unused_range) 
                    {
                        yaw_allowed = unused_range;
                    }
                }
            }
        }
    }

    // todo: make _yaw_headroom 0 to 1
    //确保偏航的范围是0-1
    yaw_allowed = MAX(yaw_allowed, (float)_yaw_headroom/1000.0f);

    if (fabsf(yaw_thrust) > yaw_allowed) 
    {
    	//限制在一个范围内
        yaw_thrust = constrain_float(yaw_thrust, -yaw_allowed, yaw_allowed);
        limit.yaw = true;
    }

    // 将偏航添加到每个电机的中间编号
    rpy_low = 0.0f;
    rpy_high = 0.0f;
    for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) 
    {
    	//判断电机是否使能
        if (motor_enabled[i]) 
        {
        	//计算推力
            _thrust_rpyt_out[i] = _thrust_rpyt_out[i] + yaw_thrust * _yaw_factor[i];

            // 记录最低横滚+俯仰+偏航指令
            if (_thrust_rpyt_out[i] < rpy_low) 
            {
                rpy_low = _thrust_rpyt_out[i];
            }
            // 记录最高横滚+俯仰+偏航指令
            if (_thrust_rpyt_out[i] > rpy_high) 
            {
                rpy_high = _thrust_rpyt_out[i];
            }
        }
    }

    //检查是否合适------ check everything fits
    throttle_thrust_best_rpy = MIN(0.5f - (rpy_low+rpy_high)/2.0, throttle_avg_max);
    if (is_zero(rpy_low) && is_zero(rpy_high))
    {
        rpy_scale = 1.0f;
    } else if (is_zero(rpy_low)) 
    {
        rpy_scale = constrain_float((1.0f-throttle_thrust_best_rpy)/rpy_high, 0.0f, 1.0f);
    } else if (is_zero(rpy_high)) 
    {
        rpy_scale = constrain_float(-throttle_thrust_best_rpy/rpy_low, 0.0f, 1.0f);
    } else 
    {
        rpy_scale = constrain_float(MIN(-throttle_thrust_best_rpy/rpy_low, (1.0f-throttle_thrust_best_rpy)/rpy_high), 0.0f, 1.0f);
    }

    // calculate how close the motors can come to the desired throttle
    //飞行员所需油门和油门推力之间的差值=pilot's desired throttle -throttle_thrust_best_rpy
    thr_adj = throttle_thrust - throttle_thrust_best_rpy;
    if (rpy_scale < 1.0f)
    {
        // Full range is being used by roll, pitch, and yaw.
        limit.roll_pitch = true;
        limit.yaw = true;
        if (thr_adj > 0.0f) 
        {
            limit.throttle_upper = true;
        }
        thr_adj = 0.0f;
    } else 
    {
        if (thr_adj < -(throttle_thrust_best_rpy+rpy_low))
        {
            // Throttle can't be reduced to desired value
        	//油门无法降低到所需值
            thr_adj = -(throttle_thrust_best_rpy+rpy_low);
        } else if (thr_adj > 1.0f - (throttle_thrust_best_rpy+rpy_high))
        {
            // Throttle can't be increased to desired value
        	//油门不能增加到期望值
            thr_adj = 1.0f - (throttle_thrust_best_rpy+rpy_high);
            limit.throttle_upper = true;
        }
    }

    // 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];
        }
    }

    //将所有输出限制为0.0f到1.0f
    //测试代码应该在这些行被注释掉的情况下运行，因为它们不应该做任何事情
    for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) 
    {
        if (motor_enabled[i]) 
        {
            _thrust_rpyt_out[i] = constrain_float(_thrust_rpyt_out[i], 0.0f, 1.0f);
        }
    }
}