混控器作用
混控器的作用是把姿态控制输出的roll,pitch,yaw力矩转换为电机的输出。PX4现将姿态控制分为几种，分别是固定翼姿态控制、无人车、无人船和多旋翼姿态控制（四轴、六轴、直升机等）。
本文代码版本是V1.13.0
混控器输入
因为混控器主要针对上游姿态环和姿态控制环产生的控制量进行二次混控，所以需要搞清楚其输入参数参数。
以多旋翼为例，位置控制和姿态控制计算出期望角速率，在MulticopterRateControl.cpp中
// run rate controller
const Vector3f att_control = _rate_control.update(rates, _rates_sp, angular_accel, dt, _maybe_landed || _landed);
// publish rate controller status
rate_ctrl_status_s rate_ctrl_status{};
_rate_control.getRateControlStatus(rate_ctrl_status);
rate_ctrl_status.timestamp = hrt_absolute_time();
_controller_status_pub.publish(rate_ctrl_status);
// publish actuator controls
actuator_controls_s actuators{};
actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f;
actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f;
actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f;
actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp) ? _thrust_sp : 0.0f;
actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _landing_gear;
actuators.timestamp_sample = angular_velocity.timestamp_sample;
// scale effort by battery status if enabled
if (_param_mc_bat_scale_en.get()) {
if (_battery_status_sub.updated()) {
battery_status_s battery_status;
if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) {
_battery_status_scale = battery_status.scale;
}
}
if (_battery_status_scale > 0.0f) {
for (int i = 0; i < 4; i++) {
actuators.control[i] *= _battery_status_scale;
}
}
}
actuators.timestamp = hrt_absolute_time();
_actuators_0_pub.publish(actuators);
该函数中发布对控制器的控制量，actuator_controls_s::中INDEX_ROLL、INDEX_PITCH、INDEX_YAW和INDEX_THROTTLE后期在混控中会根据不同的设定对输出舵机或者电机进行混控，故追踪actuator_controls_s 订阅程序，在src\drivers\pwm_out\PWMOut.cpp文件中Run函数中是对各个舵机Pwm信号输出
void PWMOut::Run()
{
if (should_exit()) {
ScheduleClear();
_mixing_output.unregister();
//exit_and_cleanup();
return;
}
perf_begin(_cycle_perf);
perf_count(_interval_perf);
// push backup schedule
ScheduleDelayed(_backup_schedule_interval_us);
_mixing_output.update();
/* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = _mixing_output.armed().armed || (_num_disarmed_set > 0) || _mixing_output.armed().in_esc_calibration_mode;
if (_pwm_on != pwm_on) {
if (update_pwm_out_state(pwm_on)) {
_pwm_on = pwm_on;
}
}
// check for parameter updates
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
// update_params(); // do not update PWM params for now (was interfering with VTOL PWM settings)
}
if (_pwm_initialized && _current_update_rate == 0) {
update_current_rate();
}
// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
_mixing_output.updateSubscriptions(true, true);
perf_end(_cycle_perf);
}
其中通过调用_mixing_output.update();函数，完成对混控的更新。追踪该函数src\lib\mixer_module\mixer_module.cpp
bool MixingOutput::update()
{
if (!_mixers) {
handleCommands();//句柄命令，如果没有完成混控加载，将会执行混控加载
// do nothing until we have a valid mixer
return false;
}
...后面代码暂时用不到
追踪handleCommands()函数，该函数是对混控的加载。src\lib\mixer_module\mixer_module.cpp
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);
}
如果Command程序选择加载_command.result = loadMixer(_command.mixer_buf, _command.mixer_buf_length);此处从混控文件加载后的buf中更新混控
该函数中读取混控执行语句如下。
int ret = _mixers->load_from_buf(controlCallback, (uintptr_t)this, buf, len);
追踪load_from_buf函数，该函数中第一个参数采用回调函数方式，Mixer::ControlCallback control_cb是一个函数指针，指向controlCallback函数。src\lib\mixer\MixerGroup.cpp
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;
/*
* Loop until either we have emptied the buffer, or we have failed to
* allocate something when we expected to.
*/
while (buflen > 0) {
Mixer *m = nullptr;
const char *p = end - buflen;
unsigned resid = buflen;
/*
* Use the next character as a hint to decide which mixer class to construct.
*/
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 we constructed something, add it to the group.
*/
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;
}
通过对_command.mixer_buf, _command.mixer_buf_length参数中加载buf中第一个有效字符不同，从而对应不同的混控加载。
Z→空混控
A→AllocatedActuatorMixer（后期PX4应该都会换成该混控加载方式）
M→简单混控
R→多旋翼
H→直升机混控
通过对ControlCallback回调函数进行解析，其主要功能就是获取当前单个设备的四个参数，handle control_group control_index control 。src\lib\mixer\MixerBase\Mixer.hpp
/**
* Fetch a control value.
*
* @param handle Token passed when the callback is registered.
* @param control_group The group to fetch the control from.
* @param control_index The group-relative index to fetch the control from.
* @param control The returned control
* @return Zero if the value was fetched, nonzero otherwise.
*/
typedef int (* ControlCallback)(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control);
/**
* Constructor.
*
* @param control_cb Callback invoked when reading controls.
*/
Mixer(ControlCallback control_cb, uintptr_t cb_handle) : _control_cb(control_cb), _cb_handle(cb_handle) {}
virtual ~Mixer() = default;
在controlCallback具体执行函数中也能找到相应的功能解释。src\lib\mixer_module\mixer_module.cpp
int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
{
const MixingOutput *output = (const MixingOutput *)handle;
input = output->_controls[control_group].control[control_index];
/* limit control input */
input = math::constrain(input, -1.f, 1.f);
/* motor spinup phase - lock throttle to zero */
if (output->_output_limit.state == OUTPUT_LIMIT_STATE_RAMP) {
if (((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
control_index == actuator_controls_s::INDEX_THROTTLE) ||
(control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART1 ||
control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART2)) {
/* limit the throttle output to zero during motor spinup,
* as the motors cannot follow any demand yet
*/
input = 0.0f;
}
}
/* throttle not arming - mark throttle input as invalid */
if (output->armNoThrottle() && !output->_armed.in_esc_calibration_mode) {
if (((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
control_index == actuator_controls_s::INDEX_THROTTLE) ||
(control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART1 ||
control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART2)) {
/* set the throttle to an invalid value */
input = NAN;
}
}
return 0;
}
本文章针对简单混控进行讲解，m = SimpleMixer::from_text(control_cb, cb_handle, p, resid);
control_cb参数是通过回调函数controlCallback进行传参。
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;
}
检测储存混控buff中的简单混控，在检测O时，如果在混控文件中没有具体表达，则使用程序中默认的参数进行设定，但是与混控文件中描写的不同的是，此处多出slew_rate_rise_time参数，查找该参数定义与使用，该参数扩展了设备的最大输出量。
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;
from_text函数中input参数读取混控文件中M:2中的混控通道数量，使用parse_control_scaler函数实现对混控中S通道加载。
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);至此混控文件加载完成。
混控器处理及输出
继续回到MixingOutput::update()函数中
bool MixingOutput::update()
{
if (!_mixers) {
handleCommands();//句柄命令，如果没有完成混控加载，将会执行混控加载
// do nothing until we have a valid mixer
return false;
}
// check arming state检查解锁状态
if (_armed_sub.update(&_armed)) {
_armed.in_esc_calibration_mode &= _support_esc_calibration;
if (_ignore_lockdown) {
_armed.lockdown = false;
}
/* Update the armed status and check that we're not locked down.
* We also need to arm throttle for the ESC calibration. */
_throttle_armed = (_armed.armed && !_armed.lockdown) || _armed.in_esc_calibration_mode;
if (_armed.armed) {
_motor_test.in_test_mode = false;
}
}
//更新输出速率
if (_param_mot_slew_max.get() > FLT_EPSILON) {
updateOutputSlewrateMultirotorMixer();
}
updateOutputSlewrateSimplemixer(); // update dt for output slew rate in simple mixer
unsigned n_updates = 0;
/* get controls for required topics */
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++;
}
/* During ESC calibration, we overwrite the throttle value. 如果在校准模式，则重写油门值*/
if (i == 0 && _armed.in_esc_calibration_mode) {
/* Set all controls to 0 */
memset(&_controls[i], 0, sizeof(_controls[i]));
/* except thrust to maximum. */
_controls[i].control[actuator_controls_s::INDEX_THROTTLE] = 1.0f;
/* Switch off the output limit ramp for the calibration. */
_output_limit.state = OUTPUT_LIMIT_STATE_ON;
}
}
}
// check for motor test (after topic updates)检测当前是否处于电机舵机测试模式
if (!_armed.armed && !_armed.manual_lockdown) {
unsigned num_motor_test = motorTest();
if (num_motor_test > 0) {
if (_interface.updateOutputs(false, _current_output_value, num_motor_test, 1)) {
actuator_outputs_s actuator_outputs{};
setAndPublishActuatorOutputs(num_motor_test, actuator_outputs);
}
handleCommands();
return true;
}
}
/* do mixing */
float outputs[MAX_ACTUATORS] {};
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);
/* overwrite outputs in case of force_failsafe with _failsafe_value values */
if (_armed.force_failsafe) {
for (size_t i = 0; i < mixed_num_outputs; i++) {
_current_output_value[i] = _failsafe_value[i];
}
}
bool stop_motors = mixed_num_outputs == 0 || !_throttle_armed;//该参数在esc输出时会进行判断
/* overwrite outputs in case of lockdown or parachute triggering with disarmed values */
//如果未解锁，则所有电机都不转
if (_armed.lockdown || _armed.manual_lockdown) {
for (size_t i = 0; i < mixed_num_outputs; i++) {
_current_output_value[i] = _disarmed_value[i];
}
stop_motors = true;
}
/* apply _param_mot_ordering 如果是beatflight则输出会发生转换*/
reorderOutputs(_current_output_value);
/* 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);
}
handleCommands();
return true;
}
通过mixed_num_outputs = _mixers->mix(outputs, _max_num_outputs);函数，完成对输出的混控。追踪该函数src\lib\mixer\MixerGroup.cpp
unsigned
MixerGroup::mix(float *outputs, unsigned space)
{
unsigned index = 0;
for (auto mixer : _mixers) {
index += mixer->mix(outputs + index, space - index);
if (index >= space) {
break;
}
}
return index;
}
根据不同的混控配置，进行混控处理，以简单混控进行设置
SimpleMixer::mix(float *outputs, unsigned space)
{
float sum = 0.0f;
if (_pinfo == nullptr) {
return 0;
}
if (space < 1) {
return 0;
}
for (unsigned i = 0; i < _pinfo->control_count; i++) {
float input = 0.0f;
//通过_control_cb函数，读取已加载的混控
_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);
if (_dt > FLT_EPSILON && _pinfo->slew_rate_rise_time > FLT_EPSILON) {
// factor 2 is needed because actuator outputs are in the range [-1,1]
const float output_delta_max = 2.0f * _dt / _pinfo->slew_rate_rise_time;
float delta_out = *outputs - _output_prev;
if (delta_out > output_delta_max) {
*outputs = _output_prev + output_delta_max;
} else if (delta_out < -output_delta_max) {
*outputs = _output_prev - output_delta_max;
}
}
// this will force the caller of the mixer to always supply dt values, otherwise no slew rate limiting will happen
_dt = 0.f;
_output_prev = *outputs;
return 1;
}
至此完成了对混控的实现，通过src\lib\mixer_module\mixer_module.cpp中update最后几行代码进行输出。
/* 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);
}
2666
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
