【Pixhawk】PX4源码控制逻辑详解(以UGV小车为例)
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
此处开启了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 ¤t_velocity);
void control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp);
在.hpp中,我们可以看出:
【模块输入】
位置期望(_manual_control_setpoint), 订阅自manual模式,即遥控控制信号
位置期望(_pos_sp_triplet), 订阅自misson模式,即自动巡航的航点
姿态期望(_att_sp)
控制模式(_control_mode)
目前位置(_global_pos)
目前姿态(_vehicle_att)
【模块输出】
控制器(_act_controls) 输出给混控器使用
(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);
}
- 位置控制通过
RoverPositionControl::control_position()函数实现
RoverPositionControl::control_position(const matrix::Vector2d ¤t_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;
}
此函数计算期望航向,期望横滚,期望推力
期望推力通过pid得出
mission_throttle = _param_throttle_speed_scaler.get()
* pid_calculate(&_speed_ctrl, mission_target_speed, x_vel, x_acc, dt);
_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;
}
}
desired_r 是图中的R (根据公式:向心加速度a = v2/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);
uORB::Publication<actuator_controls_s>
_actuator_controls_pub{ORB_ID(actuator_controls_0)}; /**< actuator controls publication */
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
- 文件名
*.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;
}
此函数将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;
}
UGV的mix以M开头,根据分支使用SimpleMixer方式,所以将调用src/lib/mixer/下的SimpleMixer模块
case 'M':
m = SimpleMixer::from_text(control_cb, cb_handle, p, resid);
break;
(4)那么我们去看看src/lib/mixer/SimpleMixer/模块做了什么
SimpleMixer.hpp
混控文件中单个控制量的格式:
O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
O: 10000 10000 0 -10000 10000
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};///10000.0f
float min_output{-1.0f};///10000.0f
float max_output{1.0f};///10000.0f
};
另外还有对输入量的描述:
/** 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 */
};
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 */
};
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;
}
(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);
}
我们知道
这里将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;
}
我们知道
该设备应具有一个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;
}
然而我们没有找到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;
}
他线程安全的调用了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);
}
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;
}
嘚,第十行,不正是我们亲切的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)},
},
(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++;
}
}
}
（3）调用混控组,outputs 为输出的 PWM
/* do mixing */
float outputs[MAX_ACTUATORS] {};
const unsigned mixed_num_outputs = _mixers->mix(outputs, _max_num_outputs);
此处调用了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);
(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);
(5)输出到硬件,_interface 可以指向 UAVCAN 或者 PWMOut
if (_interface.updateOutputs(stop_motors, _current_output_value, mixed_num_outputs, n_updates)) {...}
如果是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]);
}
}
其中调用了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);
}
因为pwm的输出本质上就是改变ccr寄存器的值
【最后】
UGV小车从接收航点,到位置姿态控制,再到调用混控输出PWM控制电机的流程大概如上,其他车驾也 按照此流程开发,中间文件和算法或有差别
若想用Pixhawk来控制麦克纳姆轮小车等固件中未定义的机型,需要自定义车驾和混控器,并选择或编写合适的算法
另外，以上逻辑均根据源码v1.12.0版本得来，不同版本结构略有差异