px4直升机混控逻辑整理

一、直升机混控矩阵的介绍：

二、源代码解读

一、直升机混控矩阵的介绍：

直升机混控：

直升机混控将三个控制输入（roll，pitch,thrust）组合为四个输出（斜盘伺服系统和主电机ESC设置）。

直升机混控的第一个输出是主电机的油门设置。随后的输出是斜盘伺服器。可以通过添加简单的混合器来控制尾桨。

推力控制输入既用于主电动机设置，也用于斜盘的总螺距。它使用油门曲线和俯仰曲线，均由五个点组成。

混合器定义以以下内容开头：

H: <number of swash-plate servos, either 3 or 4>
T: <throttle setting at thrust: 0%> <25%> <50%> <75%> <100%>
P: <collective pitch at thrust: 0%> <25%> <50%> <75%> <100%>

T:定义油门曲线的点。

P:定义总距曲线的点。

两条曲线都包含0至10000之间的五个点。对于简单的线性行为，曲线的五个值应为 0 2500 5000 7500 10000。

接下来是斜盘伺服器（3或4）的每行，格式如下：

S: <angle> <arm length> <scale> <offset> <lower limit> <upper limit>

其中 <angle>是舵机安装的角度，单位为度，0度在机头的方向上。从上方看，正角为顺时针方向。

<arm length>是臂长的比例归一化长度10000等于1。如果所有伺服臂的长度相同，则该值必须为10000。臂长越大，伺服偏转量越小，臂越短，伺服偏转量越大。

<scale>是伺服输出缩放比例，按 <scale> / 10000。缩放后，将 <offset>偏移，该范围应在-10000到+10000之间。， <lower limit>和 <upper limit>对于完整的伺服范围应当为-10000和+10000。

可以通过添加来控制尾桨简单求和混控：

M: 1
S: 0 2  10000  10000      0 -10000  10000

这样，尾桨设置将直接映射到偏航命令。这既适用于伺服控制的尾旋翼，也适用于带有专用电机的尾旋翼。

源码中有机架blade130作为示例机型：

H: 3
T:      0   3000   6000   8000  10000
P:    500   1500   2500   3500   4500
# Swash plate servos:
S:      0  10000  10000      0  -8000   8000
S:    140  13054  10000      0  -8000   8000
S:    220  13054  10000      0  -8000   8000# Tail servo:
M: 1
S: 0 2  10000  10000      0 -10000  10000

节气门曲线以稍微陡峭的坡度开始，在推力为50％时达到6000（0.6）。
它继续以较小的斜率在100％的推力下达到10000（1.0）。
总距曲线是线性的，但不会使用整个范围。
油门为0％时，总螺距设置已为500（0.05）。
在最大油门时，总螺距仅为4500（0.45）。
对这种类型的直升机使用较高的值会使叶片失速。
该直升机的斜盘舵机的角度为0度，140度和220度。
伺服臂长度不相等。
与第一伺服器相比，第二和第三伺服器的臂更长，比率为1.3054。
伺服限制在-8000和8000，因为它们受到机械限制。

二、源代码解读

直升机混控器的文件路径src/lib/mixer/HelicopterMixer/HelicopterMixer.cpp

源码贴在下面，逐步解读

unsigned
HelicopterMixer::mix(float *outputs, unsigned space)
{if (space < _mixer_info.control_count + 1u) {return 0;}/* Find index to use for curves */float thrust_cmd = get_control(0, 3);int idx = (thrust_cmd / 0.25f);/* Make sure idx is in range */if (idx < 0) {idx = 0;} else if (idx > HELI_CURVES_NR_POINTS - 2) {/* We access idx + 1 below, so max legal index is (size - 2) */idx = HELI_CURVES_NR_POINTS - 2;}/* Local throttle curve gradient and offset */float tg = (_mixer_info.throttle_curve[idx + 1] - _mixer_info.throttle_curve[idx]) / 0.25f;float to = (_mixer_info.throttle_curve[idx]) - (tg * idx * 0.25f);float throttle = constrain(2.0f * (tg * thrust_cmd + to) - 1.0f, -1.0f, 1.0f);/* Local pitch curve gradient and offset */float pg = (_mixer_info.pitch_curve[idx + 1] - _mixer_info.pitch_curve[idx]) / 0.25f;float po = (_mixer_info.pitch_curve[idx]) - (pg * idx * 0.25f);float collective_pitch = constrain((pg * thrust_cmd + po), -0.5f, 0.5f);float roll_cmd = get_control(0, 0);float pitch_cmd = get_control(0, 1);outputs[0] = throttle;for (unsigned i = 0; i < _mixer_info.control_count; i++) {outputs[i + 1] = collective_pitch+ cosf(_mixer_info.servos[i].angle) * pitch_cmd * _mixer_info.servos[i].arm_length- sinf(_mixer_info.servos[i].angle) * roll_cmd * _mixer_info.servos[i].arm_length;outputs[i + 1] *= _mixer_info.servos[i].scale;outputs[i + 1] += _mixer_info.servos[i].offset;outputs[i + 1] = constrain(outputs[i + 1], _mixer_info.servos[i].min_output, _mixer_info.servos[i].max_output);}return _mixer_info.control_count + 1;
}

首先获得油门通道值

float thrust_cmd = get_control(0, 3); 这样获得油门是在0到1之间

计算油门曲线在哪个阶段

int idx = (thrust_cmd / 0.25f); 将油门行程分为四段

确认油门在范围内

if (idx < 0) {
idx = 0;油门不允许为负数

} else if (idx > HELI_CURVES_NR_POINTS - 2) {
idx = HELI_CURVES_NR_POINTS - 2;
}
这样限幅完是0~3，下面再加1就是1~4.

接着是油门阶段曲线限制和总距曲线限制和前面定义的曲线的点是对应的

H: <number of swash-plate servos, either 3 or 4>
T: <throttle setting at thrust: 0%> <25%> <50%> <75%> <100%>
P: <collective pitch at thrust: 0%> <25%> <50%> <75%> <100%>

float tg = (_mixer_info.throttle_curve[idx + 1] - _mixer_info.throttle_curve[idx]) / 0.25f;
float to = (_mixer_info.throttle_curve[idx]) - (tg * idx * 0.25f);
float throttle = constrain(2.0f * (tg * thrust_cmd + to) - 1.0f, -1.0f, 1.0f);

/* Local pitch curve gradient and offset */
   float pg = (_mixer_info.pitch_curve[idx + 1] - _mixer_info.pitch_curve[idx]) / 0.25f;
   float po = (_mixer_info.pitch_curve[idx]) - (pg * idx * 0.25f);
   float collective_pitch = constrain((pg * thrust_cmd + po), -0.5f, 0.5f);

然后获取俯仰和横滚的通道输入

float roll_cmd = get_control(0, 0);
float pitch_cmd = get_control(0, 1);

最后将各通道将总距和俯仰以及横滚通道按舵机角度比列混控

outputs[0] = throttle;
   for (unsigned i = 0; i < _mixer_info.control_count; i++) {
       outputs[i + 1] = collective_pitch
               + cosf(_mixer_info.servos[i].angle) * pitch_cmd * _mixer_info.servos[i].arm_length
               - sinf(_mixer_info.servos[i].angle) * roll_cmd * _mixer_info.servos[i].arm_length;
       outputs[i + 1] *= _mixer_info.servos[i].scale;
       outputs[i + 1] += _mixer_info.servos[i].offset;
       outputs[i + 1] = constrain(outputs[i + 1], _mixer_info.servos[i].min_output, _mixer_info.servos[i].max_output);
   }

这里outputs[0]是油门的输出所以不用再混控。