ardupilot-3.6.9 stabilize模式下（增稳模式）油门输入输出整理

stabilize飞行模式下，飞控的油门控制直接来自于遥控器的油门通道。通过遥控器油门---->控制指令(0～1000)--->油门期望值(0～1)。

遥控器油门---->控制指令(0～1000) 

int16_t
RC_Channel::pwm_to_range_dz(uint16_t _dead_zone) const
{
    ///radio_in 遥控器输入值
    ///radio_min 遥控器最小输入值
    ///radio_max 遥控器最大输入值
    ///如果是油门通道，high_in的值是1000
    ///假设radio_min是1000,radio_max是2000，_dead_zone（死区）是30,则radio_in的范围是（1000,2000），这个函数的返回值也就是控制指令的范围是（-30,1000），正常返回应该是（−30.927835052，1000），但这个函数的除数被除数都是整型，所以舍弃小数点。
    int16_t r_in = constrain_int16(radio_in, radio_min.get(), radio_max.get());

    if (reversed) {
	    r_in = radio_max.get() - (r_in - radio_min.get());
    }

    int16_t radio_trim_low  = radio_min + _dead_zone;

    if (r_in > radio_trim_low) {
        return (((int32_t)(high_in) * (int32_t)(r_in - radio_trim_low)) / (int32_t)(radio_max - radio_trim_low));
    }
    return 0;
}

控制指令(0～1000)--->油门期望值(0～1)

// transform pilot's manual throttle input to make hover throttle mid stick
// used only for manual throttle modes
// thr_mid should be in the range 0 to 1
// returns throttle output 0 to 1
float Mode::get_pilot_desired_throttle() const
{
    const float thr_mid = throttle_hover();
    int16_t throttle_control = channel_throttle->get_control_in();

    int16_t mid_stick = copter.get_throttle_mid();
    // protect against unlikely divide by zero
    if (mid_stick <= 0) {
        mid_stick = 500;
    }

    // ensure reasonable throttle values
    throttle_control = constrain_int16(throttle_control,0,1000);

    // calculate normalised throttle input
    float throttle_in;
    if (throttle_control < mid_stick) {
        throttle_in = ((float)throttle_control)*0.5f/(float)mid_stick;
    } else {
        throttle_in = 0.5f + ((float)(throttle_control-mid_stick)) * 0.5f / (float)(1000-mid_stick);
    }

    const float expo = constrain_float(-(thr_mid-0.5f)/0.375f, -0.5f, 1.0f);
    // calculate the output throttle using the given expo function
    float throttle_out = throttle_in*(1.0f-expo) + expo*throttle_in*throttle_in*throttle_in;
    return throttle_out;
}

先说结论，这个函数完成的是，在stabilize飞行模式下，遥控器油门通道居中时，输出的期望油门(范围0~1)为油门中值（悬停时，所需油门值）。方程throttle_out是一个关于自变量throttle_in的三次多项式。下面是matlab的仿真结果。

%syms radio_min
%syms radio_max
%syms dead_zone
%syms high_in 
%syms thr_mid
%syms throttle_in

radio_min = 1000;  % 遥控器油门输入最小值
radio_max = 2000;  % 遥控器油门输入最大值
dead_zone = 30;    % 油门死区值
high_in = 1000;    % 油门变控制指令在程序中归一化0～1000
thr_mid = 0.2;     % 油门中值，多旋翼空中悬停的油门值

%模拟遥控器油门通道输入
rc_in=linspace(radio_min,radio_max,radio_max-radio_min);

r_in = (radio_min + radio_max)/2;
radio_trim_low  = radio_min + dead_zone;

%遥控器油门输入转换为控制指令
throttle_control_in = (high_in) * (rc_in - radio_trim_low) / (radio_max - radio_trim_low);

throttle_control = throttle_control_in;

%限制控制指令范围
for j=1:(radio_max-radio_min)
    if throttle_control(j) < 0
        throttle_control(j) = 0;
    elseif throttle_control(j) > 1000
        throttle_control(j) = 1000;
    end
end


%控制指令中值
mid_stick = (high_in) * (r_in - radio_trim_low) / (radio_max - radio_trim_low);

if throttle_control < mid_stick
    throttle_in = throttle_control*0.5/mid_stick;
else
    throttle_in = 0.5 + (throttle_control-mid_stick) * 0.5 / (1000-mid_stick);
end

expo = -(thr_mid-0.5)/0.375;
if expo < -0.5
    expo = -0.5;
elseif expo > 1
    expo = 1;
end

%输入到姿态控制器的期望油门值0～1
throttle_out = throttle_in.*(1.0-expo) + expo.*throttle_in.*throttle_in.*throttle_in;

subplot(1,2,1);
plot(rc_in,throttle_out);
xlabel('rc\_in')
ylabel('throttle\_out')

subplot(1,2,2);
plot(throttle_control_in,throttle_out);
xlabel('throttle\_control\_in')
ylabel('throttle\_out')

下图是仿真结果