Output_armed_stabilizing have a theoretical basis

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

//申请电压和空速计补偿----- apply voltage and air pressure compensation
const float compensation_gain = get_compensation_gain(); //补偿为电池电压高度------ compensation for battery voltage and altitude
//这个随着企业变化而变化,一般是1.08多
//得到横滚推力值=实际+前馈,注意这个前馈值不要设置太大,不然很危险
roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;

//俯仰推力值=实际+前馈
pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain;
//偏航推力值=实际+前馈
yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain;
//获取油门推力值
throttle_thrust = get_throttle() * compensation_gain;

//油门推力最大平均值0.0 - 1.0
throttle_avg_max = _throttle_avg_max * compensation_gain;

//如果推力助推激活,则不要限制最大推力----- If thrust boost is active then do not limit maximum thrust
//_thrust_boost_ratio=0,_throttle_thrust_max=1.0,正常不解锁_throttle_thrust_max=0;解锁后接变化成1
//最终throttle_thrust_max是一个大于1的数据大概是1.0几
throttle_thrust_max = _thrust_boost_ratio + (1.0f - _thrust_boost_ratio) * _throttle_thrust_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;
}

//ensure that throttle_avg_max is between the input throttle and the maximum throttle
//确保throttle_avg_max在输入油门和最大油门之间
throttle_avg_max = constrain_float(throttle_avg_max, throttle_thrust, throttle_thrust_max);

// hal.console->printf(“throttle_avg_max=%f\r\n”,throttle_avg_max);
// calculate the highest allowed average thrust that will provide maximum control range
//计算最大的平均推力:将允许提供的最大控制范围,最大不超过0.5,一般是【0 0.5】
throttle_thrust_best_rpy = MIN(0.5f, throttle_avg_max);
// hal.console->printf(“throttle_thrust_best_rpy=%f\r\n”,throttle_thrust_best_rpy);
//计算为偏航提供最大可能空间的油门值,取以下较低值:
// 1. 0.5f-(rpy_low+rpy_high)/2.0-这将提供最高电机上方和最低电机下方的最大可能裕度
// 2.较高值:
// a)飞行员油门输入值
// b)_throttle_rpy_mix:飞行员输入油门和悬停油门之间
// 情况#2确保我们绝不会将油门增加到悬停油门以上,除非飞行员已经命令这样做。
// 情况2b允许我们把油门提高到飞行员指令的高度,但不会使无人接上升。
// 如果这意味着减少发动机的油门,我们将选择#1(偏航控制的最佳油门)(即,我们倾向于减少油门*,因为*它提供更好的偏航控制)
// 只有当油门很低时,我们才会选择#2(飞行员和悬停油门的组合)。我们赞成减少油门,而不是更好的偏航控制,因为飞行员已经下令了

//在电机丢失情况下,我们从计算中删除最高电机输出,并让该电机大于1.0
//为确保控制和最大扶正性能,Hex和Octo具有一些应使用的最佳设置

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Y6 : MOT_YAW_HEADROOM = 350, ATC_RAT_RLL_IMAX = 1.0, ATC_RAT_PIT_IMAX = 1.0, ATC_RAT_YAW_IMAX = 0.5
// Octo-Quad (x8) x : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.375, ATC_RAT_PIT_IMAX = 0.375, ATC_RAT_YAW_IMAX = 0.375
// Octo-Quad (x8) + : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.75, ATC_RAT_PIT_IMAX = 0.75, ATC_RAT_YAW_IMAX = 0.375
// Usable minimums below may result in attitude offsets when motors are lost. Hex aircraft are only marginal and must be handles with care
// Hex : MOT_YAW_HEADROOM = 0, ATC_RAT_RLL_IMAX = 1.0, ATC_RAT_PIT_IMAX = 1.0, ATC_RAT_YAW_IMAX = 0.5
// Octo-Quad (x8) x : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.25, ATC_RAT_PIT_IMAX = 0.25, ATC_RAT_YAW_IMAX = 0.25
// Octo-Quad (x8) + : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.5, ATC_RAT_PIT_IMAX = 0.5, ATC_RAT_YAW_IMAX = 0.25

// 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

// Under the motor lost condition we remove the highest motor output from our calculations and let that motor go greater than 1.0
// To ensure control and maximum righting performance Hex and Octo have some optimal settings that should be used
// Y6               : MOT_YAW_HEADROOM = 350, ATC_RAT_RLL_IMAX = 1.0,   ATC_RAT_PIT_IMAX = 1.0,   ATC_RAT_YAW_IMAX = 0.5
// Octo-Quad (x8) x : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.375, ATC_RAT_PIT_IMAX = 0.375, ATC_RAT_YAW_IMAX = 0.375
// Octo-Quad (x8) + : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.75,  ATC_RAT_PIT_IMAX = 0.75,  ATC_RAT_YAW_IMAX = 0.375
// Usable minimums below may result in attitude offsets when motors are lost. Hex aircraft are only marginal and must be handles with care
// Hex              : MOT_YAW_HEADROOM = 0,   ATC_RAT_RLL_IMAX = 1.0,   ATC_RAT_PIT_IMAX = 1.0,   ATC_RAT_YAW_IMAX = 0.5
// Octo-Quad (x8) x : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.25,  ATC_RAT_PIT_IMAX = 0.25,  ATC_RAT_YAW_IMAX = 0.25
// Octo-Quad (x8) + : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.5,   ATC_RAT_PIT_IMAX = 0.5,   ATC_RAT_YAW_IMAX = 0.25
// Quads cannot make use of motor loss handling because it doesn't have enough degrees of freedom.

// 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

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//计算油门范围内的偏航量
//这始终等于或小于来自飞行员或速率控制器的请求偏航
float rp_low = 1.0f; // lowest thrust value
float rp_high = -1.0f; // highest thrust value
//最多12轴
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,剩余的值)
                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]));
            }
        }
    }
}

// calculate the maximum yaw control that can be used
// todo: make _yaw_headroom 0 to 1
//计算可使用的最大偏航控制
//todo:使_yaw_headroom从0变为1,默认_yaw_headroom=200
float yaw_allowed_min = (float)_yaw_headroom / 1000.0f;

// increase yaw headroom to 50% if thrust boost enabled
//如果启用推力助推,将偏航净空增加至50%
yaw_allowed_min = _thrust_boost_ratio * 0.5f + (1.0f - _thrust_boost_ratio) * yaw_allowed_min;

//让偏航进入最小的头部空间----- Let yaw access minimum amount of head room
yaw_allowed = MAX(yaw_allowed, yaw_allowed_min); //这里使的允许值不小于0.2

// Include the lost motor scaled by _thrust_boost_ratio to smoothly transition this motor in and out of the calculation
//包含这个丢失电机系数通过_thrust_boost_ratio以使该电机平稳地进入和退出计算


if (_thrust_boost && motor_enabled[_motor_lost_index])
{
    //记录最高横滚和俯仰命令---- record highest roll + pitch command
    if (_thrust_rpyt_out[_motor_lost_index] > rp_high)
    {
        rp_high = _thrust_boost_ratio * rp_high + (1.0f - _thrust_boost_ratio) * _thrust_rpyt_out[_motor_lost_index];
    }

    // 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[_motor_lost_index]))
    {
        if (is_positive(yaw_thrust * _yaw_factor[_motor_lost_index]))
        {
            yaw_allowed = _thrust_boost_ratio * yaw_allowed + (1.0f - _thrust_boost_ratio) * MIN(yaw_allowed, fabsf(MAX(1.0f - (throttle_thrust_best_rpy + _thrust_rpyt_out[_motor_lost_index]), 0.0f)/_yaw_factor[_motor_lost_index]));
        } else
        {
            yaw_allowed = _thrust_boost_ratio * yaw_allowed + (1.0f - _thrust_boost_ratio) * MIN(yaw_allowed, fabsf(MAX(throttle_thrust_best_rpy + _thrust_rpyt_out[_motor_lost_index], 0.0f)/_yaw_factor[_motor_lost_index]));
        }
    }
}

//限制偏航的作用范围值
if (fabsf(yaw_thrust) > yaw_allowed)
{
    //并非所有指令偏航都可以使用------ not all commanded yaw can be used
    yaw_thrust = constrain_float(yaw_thrust, -yaw_allowed, yaw_allowed);
    limit.yaw = true;
}

//增加偏航控制到推力输出---- add yaw control to thrust outputs
float rpy_low = 1.0f;   // lowest thrust value
float rpy_high = -1.0f; // highest thrust value
//最多12轴
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];

        //记录最低的横滚+俯仰+偏航命令------ record lowest roll + pitch + yaw command
        if (_thrust_rpyt_out[i] < rpy_low)
        {
            rpy_low = _thrust_rpyt_out[i];
        }
        // record highest roll + pitch + yaw command
        // Exclude any lost motors if thrust boost is enabled
        //记录最高横滚+俯仰+偏航指令
        //如果启用了推力提升,则排除任何丢失的电机
        if (_thrust_rpyt_out[i] > rpy_high && (!_thrust_boost || i != _motor_lost_index))
        {
            rpy_high = _thrust_rpyt_out[i];
        }
    }
}
// Include the lost motor scaled by _thrust_boost_ratio to smoothly transition this motor in and out of the calculation
//包含这个丢失电机系数通过_thrust_boost_ratio以使该电机平稳地进入和退出计算
if (_thrust_boost)
{
    //记录最高的横滚+俯仰+偏航命令---- record highest roll + pitch + yaw command
    if (_thrust_rpyt_out[_motor_lost_index] > rpy_high && motor_enabled[_motor_lost_index])
    {
        rpy_high = _thrust_boost_ratio * rpy_high + (1.0f - _thrust_boost_ratio) * _thrust_rpyt_out[_motor_lost_index];
    }
}

// calculate any scaling needed to make the combined thrust outputs fit within the output range
//计算使组合推力输出适合输出范围所需的任何比例
if (rpy_high - rpy_low > 1.0f)
{
    rpy_scale = 1.0f / (rpy_high - rpy_low);
}
if (throttle_avg_max + rpy_low < 0)
{
    rpy_scale = MIN(rpy_scale, -throttle_avg_max / rpy_low);
}

// calculate how close the motors can come to the desired throttle
//计算电机与所需油门的距离
rpy_high *= rpy_scale;
rpy_low *= rpy_scale;
throttle_thrust_best_rpy = -rpy_low;

// hal.console->printf(“throttle_thrust=%f\r\n”,throttle_thrust);
// hal.console->printf(“throttle_thrust_best_rpy=%f\r\n”,throttle_thrust_best_rpy);

//油门调节值
thr_adj = throttle_thrust - throttle_thrust_best_rpy;

hal.console->printf("thr_adj=%f\r\n",thr_adj);
if (rpy_scale < 1.0f)
{
    //横滚、俯仰和偏航使用全范围。 Full range is being used by roll, pitch, and yaw.
    limit.roll = true;
    limit.pitch = true;
    limit.yaw = true;
    if (thr_adj > 0.0f)
    {
        limit.throttle_upper = true;
    }
    thr_adj = 0.0f;
} else
{
    if (thr_adj < 0.0f)
    {
        // Throttle can't be reduced to desired value
        // todo: add lower limit flag and ensure it is handled correctly in altitude controller
    	//油门不能降低到所需值
    	//todo:添加下限标志并确保在高度控制器中正确处理
        thr_adj = 0.0f;
    } 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]);
    }
}

//确定谐波凹口的油门推力----- determine throttle thrust for harmonic notch
const float throttle_thrust_best_plus_adj = throttle_thrust_best_rpy + thr_adj;
//补偿增益永远不能为零---- compensation_gain can never be zero
_throttle_out = throttle_thrust_best_plus_adj / compensation_gain;

//检查故障电机----- check for failed motor
check_for_failed_motor(throttle_thrust_best_plus_adj);

}

What exactly is your question?

I don’t quite understand why this function does this

Does what?

We need exact and detailed questions in order to give proper answers.

We can not guess what is in your head. :slight_smile:

thank you very much!
I’m thinking about how he allocates the roll, pitch, yaw and throttle values here. I now encounter the problem of operating yaw and the aircraft position will move :joy:

You clearly need to check your hardware for twisted arms.
And after that do a proper PID tuning.

No need to understand nor change the source code. It is already working fine for around 2 million users. The problem here is your unique setup.

1 Like

0.5f - (rpy_low+rpy_high)/2.0
Can this explain why this is done