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]是油门的输出所以不用再混控。