【Pixhawk】PX4源码控制逻辑详解(以UGV小车为例)_px4 romfs编写机架代码和混控器代码

PX4启动流程

主体控制逻辑

• commander：飞行模式控制
• stickmapper：摇杆映射
• position_estimator：从GPS获得的位置估计
• navigator：读取航点，产生期望位置
• pos_ctrl: 位置控制
• attitude_estimator: 从IMU获得的姿态估计
• att_ctrl: 姿态控制
• mixer: 混控器根据机型分配力矩

详细控制逻辑（含数据结构体）

以UGV小车为例

1.在FIRMWARE/ROMFS/px4fmu_common/init.d/rc.rover_apps中开启了UGV小车需要用到的主要算法

#!/bin/sh
#
# Standard apps for unmanned ground vehicles (UGV).
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#

#
# Start the attitude and position estimator.
#
ekf2 start
#attitude_estimator_q start
#local_position_estimator start

#
# Start attitude controllers.
#
rover_pos_control start

#
# Start Land Detector.
#
land_detector start rover
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

此处开启了ekf2算法模块来做位置姿态估计，开启了rover_pos_control模块用于姿态位置控制，并设置工作在rover模式下，这些模块在src/modules中

2.于是从src/modules/rover_pos_control/模块入手

此模块改进自固定翼模块,所以使用了L1算法,下面包含RoverPositionControl.hpp和RoverPositionControl.cpp

(1)RoverPositionControl.hpp

	manual_control_setpoint_s		_manual_control_setpoint{}; /**< r/c channel data */
	position_setpoint_triplet_s		_pos_sp_triplet{};			/**< triplet of mission items */
	vehicle_attitude_setpoint_s		_att_sp{};					/**< attitude setpoint > */
	vehicle_control_mode_s			_control_mode{};			/**< control mode */
	vehicle_global_position_s		_global_pos{};				/**< global vehicle position */
	vehicle_local_position_s		_local_pos{};		 		/**< global vehicle position */
	actuator_controls_s				_act_controls{};			/**< direct control of actuators */
	vehicle_attitude_s				_vehicle_att{};
	vehicle_local_position_setpoint_s       _trajectory_setpoint{};
	/* Pid controller for the speed. Here we assume we can control airspeed but the control variable is actually on
	 the throttle. For now just assuming a proportional scaler between controlled airspeed and throttle output.*/
	PID_t _speed_ctrl{};
	ECL_L1_Pos_Controller				_gnd_control;
	/**
	 * Update our local parameter cache.
	 */
	void parameters_update(bool force = false);

	void		position_setpoint_triplet_poll();
	void		attitude_setpoint_poll();
	void		vehicle_control_mode_poll();
	void 		vehicle_attitude_poll();
	void		manual_control_setpoint_poll();

	/**
	 * Control position.
	 */
	bool		control_position(const matrix::Vector2d &global_pos, const matrix::Vector3f &ground_speed,
					 const position_setpoint_triplet_s &_pos_sp_triplet);
	void		control_velocity(const matrix::Vector3f &current_velocity);
	void		control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp);

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33

在.hpp中,我们可以看出:

【模块输入】

位置期望(_manual_control_setpoint), 订阅自manual模式,即遥控控制信号
位置期望(_pos_sp_triplet), 订阅自misson模式,即自动巡航的航点
姿态期望(_att_sp)
控制模式(_control_mode)
目前位置(_global_pos)
目前姿态(_vehicle_att)
1
2
3
4
5
6

【模块输出】

控制器(_act_controls) 输出给混控器使用
1

(2)RoverPositionControl.cpp

• 在RoverPositionControl::run()中分别调用了RoverPositionControl::control_attitude()和RoverPositionControl::control_position()

• 姿态控制通过RoverPositionControl::control_attitude()函数实现

RoverPositionControl::control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp)
{
	// quaternion attitude control law, qe is rotation from q to qd
	const Quatf qe = Quatf(att.q).inversed() * Quatf(att_sp.q_d);
	const Eulerf euler_sp = qe;

	float control_effort = euler_sp(2) / _param_max_turn_angle.get();
	control_effort = math::constrain(control_effort, -1.0f, 1.0f);

	_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;

	const float control_throttle = att_sp.thrust_body[0];

	_act_controls.control[actuator_controls_s::INDEX_THROTTLE] =  math::constrain(control_throttle, 0.0f, 1.0f);

}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

• 位置控制通过RoverPositionControl::control_position()函数实现

RoverPositionControl::control_position(const matrix::Vector2d &current_position,
				       const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet)
{
	float dt = 0.01; // 使用非零值来避免被零除 
	if (_control_position_last_called > 0) {
		dt = hrt_elapsed_time(&_control_position_last_called) * 1e-6f;//此刻距离上次调用的时间差值
	}
	_control_position_last_called = hrt_absolute_time();//从开机到此时的绝对时间
	bool setpoint = true;
	if ((_control_mode.flag_control_auto_enabled ||
	     _control_mode.flag_control_offboard_enabled) && pos_sp_triplet.current.valid) {
		/* 自主飞行 */
		_control_mode_current = UGV_POSCTRL_MODE_AUTO;
		/* get circle mode */
		//bool was_circle_mode = _gnd_control.circle_mode();
		/* 当前航点（当前前往的航点）*/
		matrix::Vector2d curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
		/* 前一个航点  */
		matrix::Vector2d prev_wp = curr_wp;
		if (pos_sp_triplet.previous.valid) {
			prev_wp(0) = pos_sp_triplet.previous.lat;
			prev_wp(1) = pos_sp_triplet.previous.lon;
		}
		matrix::Vector2f ground_speed_2d(ground_speed);
		float mission_throttle = _param_throttle_cruise.get();
		/* 只需控制油门 */
		if (_param_speed_control_mode.get() == 1) {
			/* 控制闭环速度  */
			float mission_target_speed = _param_gndspeed_trim.get();
			if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed) &&
			    _pos_sp_triplet.current.cruising_speed > 0.1f) {
				mission_target_speed = _pos_sp_triplet.current.cruising_speed;
			}
			// 车架速度
			const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed());
			const Vector3f vel = R_to_body * Vector3f(ground_speed(0), ground_speed(1), ground_speed(2));
			const float x_vel = vel(0);
			const float x_acc = _vehicle_acceleration_sub.get().xyz[0];
			// 计算空速控制并将其缩放为常数
			mission_throttle = _param_throttle_speed_scaler.get()
					   * pid_calculate(&_speed_ctrl, mission_target_speed, x_vel, x_acc, dt);
			// 将油门限制在最小值和最大值之间
			mission_throttle = math::constrain(mission_throttle, _param_throttle_min.get(), _param_throttle_max.get());
		} else {
			/* 只需在开环中控制油门  */
			if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_throttle) &&
			    _pos_sp_triplet.current.cruising_throttle > 0.01f) {

				mission_throttle = _pos_sp_triplet.current.cruising_throttle;
			}
		}
		float dist_target = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
				    (double)curr_wp(0), (double)curr_wp(1));
		switch (_pos_ctrl_state) {
		case GOTO_WAYPOINT: {
				if (dist_target < _param_nav_loiter_rad.get()) {
					_pos_ctrl_state = STOPPING;  // We are closer than loiter radius to waypoint, stop.

				} else {
					_gnd_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);

					_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle;

					float desired_r = ground_speed_2d.norm_squared() / math::abs_t(_gnd_control.nav_lateral_acceleration_demand());
					float desired_theta = (0.5f * M_PI_F) - atan2f(desired_r, _param_wheel_base.get());
					float control_effort = (desired_theta / _param_max_turn_angle.get()) * sign(
								       _gnd_control.nav_lateral_acceleration_demand());
					control_effort = math::constrain(control_effort, -1.0f, 1.0f);
					_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
				}
			}
			break;
		case STOPPING: {
				_act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f;
				_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
				// 注意 _prev_wp 是不同于与任务航路点相关的本地prev_wp。 
				float dist_between_waypoints = get_distance_to_next_waypoint((double)_prev_wp(0), (double)_prev_wp(1),
							       (double)curr_wp(0), (double)curr_wp(1));

				if (dist_between_waypoints > 0) {
					_pos_ctrl_state = GOTO_WAYPOINT; // A new waypoint has arrived go to it
				}
				//PX4_INFO(" Distance between prev and curr waypoints %f", (double)dist_between_waypoints);
			}
			break;

		default:
			PX4_ERR("Unknown Rover State");
			_pos_ctrl_state = STOPPING;
			break;
		}
		_prev_wp = curr_wp;
	} else {
		_control_mode_current = UGV_POSCTRL_MODE_OTHER;
		setpoint = false;
	}
	return setpoint;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98

此函数计算期望航向,期望横滚,期望推力

期望推力通过pid得出

mission_throttle = _param_throttle_speed_scaler.get()
					   * pid_calculate(&_speed_ctrl, mission_target_speed, x_vel, x_acc, dt);
1
2

_param_throttle_speed_scaler是固定参数
_speed_ctrl是存在pid参数的结构体
mission_target_speed是期望速度
x_vel是当前速度
x_acc是当前加速度
dt是时间间隔

期望航向和期望横滚需要用到L1算法,原理如下图:
期望航向即图中L1的方向(目前航点O --> 期望航点P),由于固定翼和UGV都不具备旋翼那样悬停和平动的能力,无法按照L1飞行,所以实际航线将如粗虚线所示,而这就需要改变期望横滚来实现

该函数通过调用ecl_l1_pos_controller.cpp中的navigate_waypoints得出此二项

case GOTO_WAYPOINT: {
				if (dist_target < _param_nav_loiter_rad.get()) {
					_pos_ctrl_state = STOPPING;  // We are closer than loiter radius to waypoint, stop.

				} else {
					_gnd_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);

					_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle;

					float desired_r = ground_speed_2d.norm_squared() / math::abs_t(_gnd_control.nav_lateral_acceleration_demand());
					float desired_theta = (0.5f * M_PI_F) - atan2f(desired_r, _param_wheel_base.get());
					float control_effort = (desired_theta / _param_max_turn_angle.get()) * sign(
								       _gnd_control.nav_lateral_acceleration_demand());
					control_effort = math::constrain(control_effort, -1.0f, 1.0f);
					_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
				}
			}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

desired_r 是图中的R (根据公式:向心加速度a = v²/R)
desired_theta 是期望横滚角(根据公式:Roll = arctan(a/g))
control_effort 是横滚力 = 期望横滚角/最大横滚角*加速度方向

把固定翼的L1算法搬来,用横滚来描述UGV小车的转向并不合适,所以这里的最后一行就将横滚值给了偏航,小车所用参数只有偏航actuator_controls_s::INDEX_YAW和油门actuator_controls_s::INDEX_THROTTLE

• 以上得到的控制信息全都通过_act_controls发布到Group#0输入组中

_actuator_controls_pub.publish(_act_controls);
1

uORB::Publication<actuator_controls_s>		
_actuator_controls_pub{ORB_ID(actuator_controls_0)};  /**< actuator controls publication */
1
2

3.混控文件如何被设备调用

(1)混控文件:Firmware/ROMFS/px4fmu_common/mixers/rover_generic.main.mix

Generic car mixer (eg Traxxas Stampede RC Car)
===========================

Designed for Traxxas Stampede

This file defines mixers suitable for controlling a Traxxas Stampede rover using
PX4FMU. The configuration assumes the steering is connected to PX4FMU
servo outputs  1 and the motor speed control to output 3. Output 0 and 2 is
assumed to be unused.

Inputs to the mixer come from channel group 0 (vehicle attitude), channels 2 (yaw), and 3 (thrust).

See the README for more information on the scaler format.

Output 1: Empty
---------------------------------------
Z:

Output 2: Steering mixer using yaw, with 0.5s rise time
---------------------------------------
M: 1
O: 10000 10000 0 -10000 10000 5000
S: 0 2   10000   10000      0 -10000  10000


Output 3: Empty
---------------------------------------
This mixer is empty.
Z:

Output 4: Throttle with 2s rise time
---------------------------------------
M: 1
O: 10000 10000 0 -10000 10000 20000
S: 0 3  10000  10000      0 -10000  10000
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

• 文件名*.main.mix 与 *.aux.mix分别表示由IO还是FMU输出PWM
• 文件中Output与真实的PWM输出顺序一致,这里只用到了PWM2和PWM4; PWM1,PWM3为空混控器
• PWM2输出偏航到伺服电机来转向(转动下图圆形部分),PWM4输出油门给两个黄色的主动轮电机来变速,另外还有四个红色的随动轮不受pixhawk控制
  

(2)Firmware/src/lib/mixer/load_mixer_file.cpp使用load_mixer_file()加载mix文件

int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
{
	FILE *fp;
	char line[120];
	/* open the mixer definition file */
	fp = fopen(fname, "r");
	if (fp == nullptr) {
		printf("file not found\n");
		return -1;
	}
	/* read valid lines from the file into a buffer */
	buf[0] = '\0';
	for (;;) {
		/* get a line, bail on error/EOF */
		line[0] = '\0';
		if (fgets(line, sizeof(line), fp) == nullptr) {
			break;
		}
		/* if the line doesn't look like a mixer definition line, skip it */
		if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) {
			continue;
		}
		/* compact whitespace in the buffer */
		char *t, *f;
		for (f = line; *f != '\0'; f++) {
			/* scan for space characters */
			if (*f == ' ') {
				/* look for additional spaces */
				t = f + 1;
				while (*t == ' ') {
					t++;
				}
				if (*t == '\0') {
					/* strip trailing whitespace */
					*f = '\0';

				} else if (t > (f + 1)) {
					memmove(f + 1, t, strlen(t) + 1);
				}
			}
		}
		/* if the line is too long to fit in the buffer, bail */
		if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
			printf("line too long\n");
			fclose(fp);
			return -1;
		}
		/* add the line to the buffer */
		strcat(buf, line);
	}
	fclose(fp);
	return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53

此函数将mix文件加载到缓冲区,供具体设备处理

(3)Firmware/src/lib/mixer/MixerGroup.cpp,定义了load_from_buf()函数

顾名思义,将3.(2)提到的缓冲区进行处理

例如,根据mix文件内容选择不同的混控方式,src/lib/mixer/下有混控方式的类型,如下所示:

int
MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
{
	int ret = -1;
	const char *end = buf + buflen;
	while (buflen > 0) {
		Mixer *m = nullptr;
		const char *p = end - buflen;
		unsigned resid = buflen;
		switch (*p) {
		case 'Z':
			m = NullMixer::from_text(p, resid);
			break;
		case 'A':
			m = AllocatedActuatorMixer::from_text(control_cb, cb_handle, p, resid);
			break;
		case 'M':
			m = SimpleMixer::from_text(control_cb, cb_handle, p, resid);
			break;
		case 'R':
			m = MultirotorMixer::from_text(control_cb, cb_handle, p, resid);
			break;
		case 'H':
			m = HelicopterMixer::from_text(control_cb, cb_handle, p, resid);
			break;
		default:
			/* it's probably junk or whitespace, skip a byte and retry */
			buflen--;
			continue;
		}
		if (m != nullptr) {
			add_mixer(m);
			/* we constructed something */
			ret = 0;
			/* only adjust buflen if parsing was successful */
			buflen = resid;
			debug("SUCCESS - buflen: %d", buflen);
		} else {
			break;
		}
	}
	return ret;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43

UGV的mix以M开头,根据分支使用SimpleMixer方式,所以将调用src/lib/mixer/下的SimpleMixer模块

		case 'M':
			m = SimpleMixer::from_text(control_cb, cb_handle, p, resid);
			break;
1
2
3

(4)那么我们去看看src/lib/mixer/SimpleMixer/模块做了什么

SimpleMixer.hpp

混控文件中单个控制量的格式:

O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
O: 10000       10000       0        -10000        10000         
1
2

SimpleMixer.hpp中的结构体对单个控制量格式的描述:

/** simple channel scaler */
struct mixer_scaler_s {
	float negative_scale{1.0f};//<-ve scale>/10000.0f
	float positive_scale{1.0f};//<+ve scale>/10000.0f
	float offset{0.0f};//<offset>/10000.0f
	float min_output{-1.0f};//<lower limit>/10000.0f
	float max_output{1.0f};//<upper limit>/10000.0f
};
1
2
3
4
5
6
7
8

另外还有对输入量的描述:

/** mixer input */
struct mixer_control_s {
	uint8_t			control_group;	/**< group from which the input reads */
	uint8_t			control_index;	/**< index within the control group */
	mixer_scaler_s		scaler;		/**< scaling applied to the input before use */
};
1
2
3
4
5
6

control_group是控制输入组,即S:后的第1个数字
control_index是控制输入组元素,即S:后的第2个数字

对控制实体的描述:

/** simple mixer */
struct mixer_simple_s {
	uint8_t			control_count;	/**< number of inputs */
	mixer_scaler_s		output_scaler;	/**< scaling for the output */
	float 			slew_rate_rise_time{0.0f}; /**< output max rise time (slew rate limit)*/
	mixer_control_s		controls[];	/**< actual size of the array is set by control_count */
};
1
2
3
4
5
6
7

control_count是输入信号的数量,即混控文件中M:后面的数字
controls[]是具体的信号
output_scaler是输出

SimpleMixer.cpp

这里就定义了from_text(),即对以M:开头的mix文件进行解析,并将解析到的参数载入上述结构体

SimpleMixer *
SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
{
	SimpleMixer *sm = nullptr;
	mixer_simple_s *mixinfo = nullptr;
	unsigned inputs;
	int used;
	const char *end = buf + buflen;
	char next_tag;
	/* enforce that the mixer ends with a new line */
	if (!string_well_formed(buf, buflen)) {
		return nullptr;
	}
	/* get the base info for the mixer */
	if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) {
		debug("simple parse failed on '%s'", buf);
		goto out;
	}
	/* at least 1 input is required */
	if (inputs == 0) {
		debug("simple parse got 0 inputs");
		goto out;
	}
	buf = skipline(buf, buflen);

	if (buf == nullptr) {
		debug("no line ending, line is incomplete");
		goto out;
	}
	mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));

	if (mixinfo == nullptr) {
		debug("could not allocate memory for mixer info");
		goto out;
	}
	mixinfo->control_count = inputs;
	/* find the next tag */
	next_tag = findnexttag(end - buflen, buflen);
	if (next_tag == 'S') {
		/* No output scalers specified. Use default values.
		 * Corresponds to:
		 * O:      10000  10000      0 -10000  10000 0
		 */
		mixinfo->output_scaler.negative_scale	= 1.0f;
		mixinfo->output_scaler.positive_scale	= 1.0f;
		mixinfo->output_scaler.offset		= 0.f;
		mixinfo->output_scaler.min_output	= -1.0f;
		mixinfo->output_scaler.max_output	= 1.0f;
		mixinfo->slew_rate_rise_time		= 0.0f;
	} else {
		if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler, mixinfo->slew_rate_rise_time)) {
			debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf);
			goto out;
		}
	}
	for (unsigned i = 0; i < inputs; i++) {
		if (parse_control_scaler(end - buflen, buflen,
					 mixinfo->controls[i].scaler,
					 mixinfo->controls[i].control_group,
					 mixinfo->controls[i].control_index)) {
			debug("simple mixer parser failed parsing ctrl scaler tag, ret: '%s'", buf);
			goto out;
		}
	}

	sm = new SimpleMixer(control_cb, cb_handle, mixinfo);

	if (sm != nullptr) {
		mixinfo = nullptr;
		debug("loaded mixer with %d input(s)", inputs);
	} else {
		debug("could not allocate memory for mixer");
	}
out:
	if (mixinfo != nullptr) {
		free(mixinfo);
	}
	return sm;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79

(5)我们现在看设备如何调用我们解析的数据

根据src/systemcmds/mixer/mixer.cpp中
load()函数的定义

static int 
load(const char *devname, const char *fname, bool append)
	char buf[2048];

	if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) {
		PX4_ERR("can't load mixer file: %s", fname);
		return 1;
	}

	/* Pass the buffer to the device */
	int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf);
}
1
2
3
4
5
6
7
8
9
10
11
12

我们知道
这里将mix文件通过load_mixer_file()函数加载到缓冲区buf[2048],设备将缓冲区作为入参,不难猜到,是为了设备方便调用load_from_buf()来处理此缓冲区,我们一探便知

如何探?
根据最后一行加载buf的px4_ioctl()函数的定义

	int px4_ioctl(int fd, int cmd, unsigned long arg)
	{
		PX4_DEBUG("px4_ioctl fd = %d", fd);
		int ret = 0;
		cdev::CDev *dev = getFile(fd);
		if (dev) {
			ret = dev->ioctl(&filemap[fd], cmd, arg);
		} else {
			ret = -EINVAL;
		}
		return ret;
	}
1
2
3
4
5
6
7
8
9
10
11
12

我们知道
该设备应具有一个ioctl()函数供调用

比如,下面就是src/drivers/uavcan/uavcan_main.cpp中的ioctl()函数,将在这里调用load_from_buf()

int
UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
{
	int ret = OK;
	lock();
	switch (cmd) {
	case PWM_SERVO_SET_ARM_OK:
	case PWM_SERVO_CLEAR_ARM_OK:
	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
		// these are no-ops, as no safety switch
		break;
	case MIXERIOCRESET:
		_mixing_interface.mixingOutput().resetMixerThreadSafe();
		break;
	case MIXERIOCLOADBUF: {
			const char *buf = (const char *)arg;
			unsigned buflen = strlen(buf);
			ret = _mixing_interface.mixingOutput().loadMixerThreadSafe(buf, buflen);
		}
		break;
	case UAVCAN_IOCS_HARDPOINT_SET: {
			const auto &hp_cmd = *reinterpret_cast<uavcan::equipment::hardpoint::Command *>(arg);
			_hardpoint_controller.set_command(hp_cmd.hardpoint_id, hp_cmd.command);
		}
		break;
	case UAVCAN_IOCG_NODEID_INPROGRESS: {
			UavcanServers   *_servers = UavcanServers::instance();
			if (_servers == nullptr) {
				// status unavailable
				ret = -EINVAL;
				break;
			} else if (_servers->guessIfAllDynamicNodesAreAllocated()) {
				// node discovery complete
				ret = -ETIME;
				break;
			} else {
				// node discovery in progress
				ret = OK;
				break;
			}
		}
	default:
		ret = -ENOTTY;
		break;
	}
	unlock();
	if (ret == -ENOTTY) {
		ret = CDev::ioctl(filp, cmd, arg);
	}
	return ret;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51

然而我们没有找到load_from_buf(),不过根据cmd的值,我们知道调用了loadMixerThreadSafe()

loadMixerThreadSafe()何许人也???
我们在src/lib/mixer_module/mixer_module.cpp中找到了他

int MixingOutput::loadMixerThreadSafe(const char *buf, unsigned len)
{
	if ((Command::Type)_command.command.load() != Command::Type::None) {
		// Cannot happen, because we expect only one other thread to call this.
		// But as a safety precaution we return here.
		PX4_ERR("Command not None");
		return -1;
	}
	lock();
	_command.mixer_buf = buf;
	_command.mixer_buf_length = len;
	_command.command.store((int)Command::Type::loadMixer);
	_interface.ScheduleNow();
	unlock();
	// wait until processed
	while ((Command::Type)_command.command.load() != Command::Type::None) {
		usleep(1000);
	}
	return _command.result;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

他线程安全的调用了loadMixer()

void MixingOutput::handleCommands()
{
	if ((Command::Type)_command.command.load() == Command::Type::None) {
		return;
	}
	switch ((Command::Type)_command.command.load()) {
	case Command::Type::loadMixer:
		_command.result = loadMixer(_command.mixer_buf, _command.mixer_buf_length);
		break;
	case Command::Type::resetMixer:
		resetMixer();
		_command.result = 0;
		break;
	default:
		break;
	}
	// mark as done
	_command.command.store((int)Command::Type::None);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

loadMixer()何许人也???
也在该文件中

int MixingOutput::loadMixer(const char *buf, unsigned len)
{
	if (_mixers == nullptr) {
		_mixers = new MixerGroup();
	}
	if (_mixers == nullptr) {
		_groups_required = 0;
		return -ENOMEM;
	}
	int ret = _mixers->load_from_buf(controlCallback, (uintptr_t)this, buf, len);
	if (ret != 0) {
		PX4_ERR("mixer load failed with %d", ret);
		delete _mixers;
		_mixers = nullptr;
		_groups_required = 0;
		return ret;
	}
	_mixers->groups_required(_groups_required);
	PX4_DEBUG("loaded mixers \n%s\n", buf);
	updateParams();
	_interface.mixerChanged();
	return ret;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

嘚,第十行,不正是我们亲切的load_from_buf()嘛

4.那么问题来了:

我们在文章第2部分通过姿态控制得到的控制量全都通过_act_controls{}发布到了Group#0输入组中,那么怎么使我们的控制量生效呢?

在src/lib/mixer_module/mixer_module.cpp中

(1)_control_subs包含了对六个输入组的订阅

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)},
},
1
2
3
4
5
6
7
8
9
10
11
12

(2)订阅Group#0输入组的控制量

	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) 
	{
		if (_groups_subscribed & (1 << i)) {
			if (_control_subs[i].copy(&_controls[i])) {
				n_updates++;
			}
		}
	}
1
2
3
4
5
6
7
8

（3）调用混控组,outputs 为输出的 PWM

	/* do mixing */
	float outputs[MAX_ACTUATORS] {};
	const unsigned mixed_num_outputs = _mixers->mix(outputs, _max_num_outputs);
1
2
3

此处调用了SimpleMixer.cpp --> mix(float *outputs, unsigned space)来叠加输入量到 PWM

	for (unsigned i = 0; i < _pinfo->control_count; i++) {
		float input = 0.0f;

		_control_cb(_cb_handle,
			    _pinfo->controls[i].control_group,
			    _pinfo->controls[i].control_index,
			    input);

		sum += scale(_pinfo->controls[i].scaler, input);
	}

	*outputs = scale(_pinfo->output_scaler, sum);
1
2
3
4
5
6
7
8
9
10
11
12

(4)矫正PWM: [-1, 1] --> [-1000, 1000],输出到_current_output_value

/* 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);
1
2
3

(5)输出到硬件,_interface 可以指向 UAVCAN 或者 PWMOut

if (_interface.updateOutputs(stop_motors, _current_output_value, mixed_num_outputs, n_updates)) {...}
1

如果是PWM输出
则updateOutputs调用的是src/drivers/pwm_out/PWMOut.cpp中的updateOutputs

/* 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]);
	}
}
1
2
3
4
5
6

其中调用了platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c中的up_pwm_servo_set

int up_pwm_servo_set(unsigned channel, servo_position_t value)
{
	return io_timer_set_ccr(channel, value);
}
1
2
3
4

因为pwm的输出本质上就是改变ccr寄存器的值

---

【最后】

UGV小车从接收航点,到位置姿态控制,再到调用混控输出PWM控制电机的流程大概如上,其他车驾也 按照此流程开发,中间文件和算法或有差别
若想用Pixhawk来控制麦克纳姆轮小车等固件中未定义的机型,需要自定义车驾和混控器,并选择或编写合适的算法

另外，以上逻辑均根据源码v1.12.0版本得来，不同版本结构略有差异