PX4的控制与输出

---

前言

---

这篇记录一下自己分析PX4从位置控制->速度控制->姿态控制->角速度控制->电机输出的源码框架的阅读与理解，便于之后自己修改控制器。在这个部分前面的navigator和commander模块不在本次谈论范围之内。

---

控制器部分

---

首先说明一下，在PX4的代码中，无人机的位置控制和速度控制的代码并不是分开的而是都在mc_pos_control里，毕竟位置控制就是一个P控制，速度控制也就是一个常规的PID控制，确实也没啥好写的。

首先对于阅读过ardupilot的控制框架的我来说，PX4的源代码无疑清爽了很多，不同的应用模块之间使用urob进行通讯，使得他们之间不再有相互耦合。所以大部分情况，我们只需要在.h文件中找到订阅和发布的话题，就可以用它们方便的找到上下相互配合的应用模块。一开始，我发现有些模块里：比如mc_pos_control里发布了vehicle_attitude_setpoint话题,而mc_att_control里也发布了vehicle_attitude_setpoint话题，理所应当，它也订阅了这个话题。当时我就一下整不会了？别的模块发，然后它自己也发？那他订阅的时候是用的那一个，而且为啥会自己给自己发呢？带着疑惑，我继续往下读，然后直到看到下面几行代码，我明白了。

// Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode
if (_v_control_mode.flag_control_manual_enabled &&
	 !_v_control_mode.flag_control_altitude_enabled &&
	 !_v_control_mode.flag_control_velocity_enabled &&
	 !_v_control_mode.flag_control_position_enabled) {

		 generate_attitude_setpoint(q, dt, _reset_yaw_sp);
		 attitude_setpoint_generated = true;

		} else {
				_man_x_input_filter.reset(0.f);
				_man_y_input_filter.reset(0.f);
		}

是的，这里mc_att_control想要发布vehicle_attitude_setpoint是有条件的，需要进行一个判断我们现在是处于手动模式下（PX4 四旋翼机型的manual和stabilized模式是完全一样的，只有固定翼下二者才有区别），也就是说mc_pos_control此时是不工作的，这个时候mc_att_control才会自己根据我们的摇杆输入来来生成姿态控制指令并发布然后自己又订阅来进行下一步处理，mc_rate_control也是相似的。

说完这些我们先看看姿态控制和角速度控制部分，因为这两个部分的代码条理都非常清晰，姿态控制mc_att_control采用四元数控制生成姿态角速度的控制指令并发布相应的vehicle_rates_setpoint话题，角速度控制mc_rate_control常规的PID控制来生成力矩的控制指令并发布相应的话题actuator_controls_0。以上控制的具体框架和说明，到PX4官方开发者手册查看即可，写的很清楚明白，这里就不再赘述。

---

输出部分

---

控制组

到这里就数据已经发布到了控制组0中，我们可以在PX4中找到控制组0的具体定义。对于四旋翼的控制而言，我们基本只用看控制组0即可，其他的控制组可以先不管。

Control Group #0 (Flight Control)
•	0: roll (-1..1)
•	1: pitch (-1..1)
•	2: yaw (-1..1)
•	3: throttle (0..1 normal range, -1..1 for variable pitch / thrust reversers)
•	4: flaps (-1..1)
•	5: spoilers (-1..1)
•	6: airbrakes (-1..1)
•	7: landing gear (-1..1)

从以上定义我们可以看到，控制器最终的输出被映射到（-1，1）区间中，那么接下来我们就需要将利用这些控制信号以及mixer混控器来产生最终的实际信号输出。

混控器

混控器说简单它可以很简单，说复杂也可以很复杂。本质上来说，它的核心作用就是把控制器输出的yaw,pitch,roll,thrust等控制量根据实际输出的执行器产生的作用矩阵将其进行实际的输出分配。所以它的本质不过就是一个变换矩阵而已。PX4中一共有4种混控器类型：R,H,M,Z。他们分别代表了多旋翼混控器，直升机构型混控器，简单加法混控器，空混控器。我们可以看看脚本语法：

M: <control count>
O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit> <traversal time>
S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>

• 脚本中只有以 大写字母 + ：的组合的开头的一行才会生效，别的默认都是注释。

• M：后紧跟的数字表示是该通道由多少个输入源混合而成

• O: 表示输出的一个幅值约束，最后是一个移动时间可选项，比如我们用舵机控制一个起落架，那么显然可以把整个值设置大一些。这一项如果不写的话，则默认为：
  O: 10000 10000 0 -10000 10000

• S: 表示是输出的信号来源 <控制组别号> <控制量索引号>后面也是一些限幅约束

R: <geometry> <roll scale> <pitch scale> <yaw scale> <idlespeed>

支持的几何构型<geometry> 包括：

• 4x : quadrotor in X configuration
• 4+ : quadrotor in + configuration
• 6x : hexacopter in X configuration
• 6+ : hexacopter in + configuration
• 8x : octocopter in X configuration
• 8+ : octocopter in + configuration

Z:

空mixer用来占位，表示该通道什么也不输出。脚本中写每一个混空按照顺序依次映射到实际的1，2，3，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%>
S: <angle> <arm length> <scale> <offset> <lower limit> <upper limit>

整个由于目前我不用所以就先不写了。

这里简单有兴趣继续深究的，可以去看官网，要是官网看不明白可以去看阿木实验室的混控器课程或者大佬文章1以及大佬文章2

---

混控器的启动

混控器的启动是在启动脚本rcS的运行下一步步加载的。大概的加载顺序是rcS->rc.autostart->4001_quad_x->rc.mc_defaults
在4001_quad_x中设置了set MIXER quad_x 其中quad_x 就对应了脚本quad_x.main.mix，其内容如下：

R: 4x
M: 1
S: 3 5 10000 10000 0 -10000 10000
M: 1
S: 3 6 10000 10000 0 -10000 10000
Z:
Z:

在rc.mc_defaults设置了set MIXER_AUX pass 其中pass对应了脚本pass.aux.mix，其内容如下：

M: 1
S: 3 5 10000 10000 0 -10000 10000
M: 1
S: 3 6 10000 10000 0 -10000 10000
M: 1
S: 3 7 10000 10000 0 -10000 10000
M: 1
S: 3 4 10000 10000 0 -10000 10000

多旋翼和固定翼默认装载 pass.aux.mix (如下所示)将遥控器映射到辅助通道输出。

pass.aux.mix的注释如下：

# Manual pass through mixer for servo outputs 1-4

# AUX1 channel (select RC channel with RC_MAP_AUX1 param)
M: 1
S: 3 5  10000  10000      0 -10000  10000

# AUX2 channel (select RC channel with RC_MAP_AUX2 param)
M: 1
S: 3 6  10000  10000      0 -10000  10000

# AUX3 channel (select RC channel with RC_MAP_AUX3 param)
M: 1
S: 3 7  10000  10000      0 -10000  10000

# FLAPS channel (select RC channel with RC_MAP_FLAPS param)
M: 1
S: 3 4  10000  10000      0 -10000  10000

完成上述设置后，之后rcS会执行/etc/init.d/rc.interface 脚本。在该脚本中，将根据之前的设置参数正式加载quad_x.main.mix 和pass.aux.mix两个脚本，至此混控器就完成启动了

---

信号输出

在信号输出的最后阶段我们就有两种情况，一种情况是飞控用协处理器px4io输出最终控制信号，一种情况是飞控FMU直接输出最终控制信号。第一种情况，有大佬文章说的很清楚，阿木实验室的混控器课程也讲的非常清楚，这里我不再赘述。

这里我想主要记录一下第二种情况，这种情况也是目前大家做四旋翼无人用的最多的情况。电机控制信号直接由FMU输出，信号的速率不仅更快一些，而且开发也更简洁一点。

这里我们主要说PWM的输出的，DSHOT的输出显然同理，不再赘述。
这里主要涉及到了到src/drivers/pwm_out，src/lib/mixer，src/lib/mixer_module这三个文件（更底层的一些系统操作我们就先不关注了）。

首先需要说明的是在lib里的程序都不会属于应用程序，所以说这么一说，大家自然就很清楚了，必然是pwm_out的程序是作为进程执行，而mixer和mixer_module中的类和函数之类的则在pwm_out中被定义被调用。

src/lib/mixer文件中有各种mixer的定义和程序实现，有兴趣的朋友请移步这篇文章（这篇文章对混控器代码和混控脚本都作了详细说明）。建议大家阅读，PX4中混控器不仅通过效率矩阵将控制力和控制力矩分配到各个电机，而且还包含了输出抗饱和以及旋翼动力与PWM的映射关系，这些都会直接影响到最后的控制效果，而不是说我们只关注前面控制器设计就可以了。当然了，就目前来说，它的效果是够用的，我们改控制器时可以不用关心这里，但是如果说我们要设计一个对旋翼模型进行参数辨识的较为准确的拉力模型后，这个地方也需要我们自己改一改了。

src/lib/mixer_module基本上可以说是为上层应用调用mixer做好各种方便的函数接口，尤其是里面MixingOutput的构造。之前我们说到过，最终的电机输出的信号取决于actuator_controls_0的控制指令和mixer混控器两个东西。那么在下面的构造函数中我们可以看到，这里显然对actuator_controls_0的话题进行订阅的操作。这个构造具体的实现我没太明白，不过可以明确的是这个类的成员中一定拥有了actuator_controls_0的数据。

MixingOutput::MixingOutput(uint8_t max_num_outputs, OutputModuleInterface &interface,
			   SchedulingPolicy scheduling_policy,
			   bool support_esc_calibration, bool ramp_up)
	: ModuleParams(&interface),
	  _control_subs{
	{&interface, ORB_ID(actuator_controls_0)},
	{&interface, ORB_ID(actuator_controls_1)},
	{&interface, ORB_ID(actuator_controls_2)},
	{&interface, ORB_ID(actuator_controls_3)},
	{&interface, ORB_ID(actuator_controls_4)},
	{&interface, ORB_ID(actuator_controls_5)},
}, // 部分构造函数

接下来要说明的src/lib/mixer_module中的update函数，这个函数非常重要，它完成了许多重要的工作。这里我贴出部分重要代码，大家再看这些关键注释就可以很清楚了。

/* get controls for required topics */
if (_control_subs[i].copy(&_controls[i])) //这里就复制了控制组信号到_controls
/* do mixing */
const unsigned mixed_num_outputs = _mixers->mix(outputs, _max_num_outputs);
/* the output limit call takes care of out of band errors, NaN and constrains */
output_limit_calc(_throttle_armed, armNoThrottle(), mixed_num_outputs, _reverse_output_mask,_disarmed_value, _min_value, _max_value, outputs, _current_output_value, &_output_limit);
/* now return the outputs to the driver */
if (_interface.updateOutputs(stop_motors, _current_output_value, mixed_num_outputs, n_updates)) {
		actuator_outputs_s actuator_outputs{};
		setAndPublishActuatorOutputs(mixed_num_outputs, actuator_outputs);

		publishMixerStatus(actuator_outputs);
		updateLatencyPerfCounter(actuator_outputs);
	}

• 获得控制量
• 进行混控
• 进行输出限制幅处理
• 将输出返回到驱动（这里的驱动当然就是pwm,dshot驱动之类的了）

我们可以看到这里是通过下面这个函数更新了输出的电机控制信号值

_interface.updateOutputs(stop_motors, _current_output_value, mixed_num_outputs, n_updates)

这个函数在最初的 OutputModuleInterface类中是一个虚函数，它将在 OutputModuleInterface的派生类PWMout中被重新定义，也就赋予了它实际的输出功能，如下所示。当然作为一个输出接口，它也可以同样被dshot重定义从而输出dshot信号。

bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
			   unsigned num_outputs, unsigned num_control_groups_updated)
{
	if (_test_mode) {
		return false;
	}

	/* output to the servos */
	if (_pwm_initialized) {
		for (size_t i = 0; i < math::min(_num_outputs, num_outputs); i++) {
			up_pwm_servo_set(_output_base + i, outputs[i]);
		}
	}

	/* Trigger all timer's channels in Oneshot mode to fire
	 * the oneshots with updated values.
	 */
	if (num_control_groups_updated > 0) {
		up_pwm_update(); // TODO: review for multi
	}

	return true;
}

就在这里，调用了来自于操作系统中的up_pwm_servo_set(unsigned channel, servo_position_t value)函数，该函数再进一步调用底层函数int io_timer_set_ccr(unsigned channel, uint16_t value)将对应的pwm输出写入对应通道的CCR寄存器中，玩过stm32的朋友应该很清楚这个寄存器就是产生pwm的寄存器。

上述说描述的src/lib/mixer_module的重要函数，最终在src/drivers/pwm_out中的Run函数中一次调用中完成：

_mixing_output.update();

至此PX4整个控制和输出的过程的分析说明便画上句号。