Refer to 1.10 firmware. Take four rotors for example. If there is any omission, please correct it
The function of the hybrid controller is to convert the roll, pitch and yaw moment of the attitude control output into the output of the motor. Px4 groups the input of the hybrid controller, refer to https://dev.px4.io/master/en/concept/mixing.html
Each group has eight values. The values in different groups may be different. They are used to achieve different functions. The attitude control of the four rotors uses group0. The first four values are mainly used, namely roll,pitch,yaw,thr. These are the expected torque and thrust.
These are generated by attitude control, specifically in the following function
void
MulticopterAttitudeControl::publish_actuator_controls()
{
_actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;
_actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.control[7] = (float)_landing_gear.landing_gear;
// note: _actuators.timestamp_sample is set in MulticopterAttitudeControl::Run()
_actuators.timestamp = hrt_absolute_time();
/* scale effort by battery status */ if (_param_mc_bat_scale_en.get() && _battery_status.scale > 0.0f) { for (int i = 0; i < 4; i++) { _actuators.control[i] *= _battery_status.scale; } } if (!_actuators_0_circuit_breaker_enabled) { orb_publish_auto(_actuators_id, &_actuators_0_pub, &_actuators, nullptr, ORB_PRIO_DEFAULT); }
}
The first four values are_ att_control(0),_att_control(1),_att_control(2) and_ thrust_sp
The desired thrust is generated by the remote control
attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z);
A kind of att_ The control is generated by the attitude control of the inner loop
_att_control = _rate_control.update(rates, _rates_sp, dt, landed);
After the group 0 values are published in the attitude control module, the hybrid controller will subscribe these values through the callback function
Look at this function
unsigned
MultirotorMixer::mix(float *outputs, unsigned space)
{
if (space < _rotor_count) {
return 0;
}
float roll = math::constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f); float pitch = math::constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f); float yaw = math::constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f); float thrust = math::constrain(get_control(0, 3), 0.0f, 1.0f); // clean out class variable used to capture saturation _saturation_status.value = 0; // Do the mixing using the strategy given by the current Airmode configuration switch (_airmode) { case Airmode::roll_pitch: mix_airmode_rp(roll, pitch, yaw, thrust, outputs); break; case Airmode::roll_pitch_yaw: mix_airmode_rpy(roll, pitch, yaw, thrust, outputs); break; case Airmode::disabled: default: // just in case: default to disabled mix_airmode_disabled(roll, pitch, yaw, thrust, outputs); break; } // Apply thrust model and scale outputs to range [idle_speed, 1]. // At this point the outputs are expected to be in [0, 1], but they can be outside, for example // if a roll command exceeds the motor band limit. for (unsigned i = 0; i < _rotor_count; i++) { // Implement simple model for static relationship between applied motor pwm and motor thrust // model: thrust = (1 - _thrust_factor) * PWM + _thrust_factor * PWM^2 if (_thrust_factor > 0.0f) { outputs[i] = -(1.0f - _thrust_factor) / (2.0f * _thrust_factor) + sqrtf((1.0f - _thrust_factor) * (1.0f - _thrust_factor) / (4.0f * _thrust_factor * _thrust_factor) + (outputs[i] < 0.0f ? 0.0f : outputs[i] / _thrust_factor)); } outputs[i] = math::constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f); } // Slew rate limiting and saturation checking for (unsigned i = 0; i < _rotor_count; i++) { bool clipping_high = false; bool clipping_low_roll_pitch = false; bool clipping_low_yaw = false; // Check for saturation against static limits. // We only check for low clipping if airmode is disabled (or yaw // clipping if airmode==roll/pitch), since in all other cases thrust will // be reduced or boosted and we can keep the integrators enabled, which // leads to better tracking performance. if (outputs[i] < _idle_speed + 0.01f) { if (_airmode == Airmode::disabled) { clipping_low_roll_pitch = true; clipping_low_yaw = true; } else if (_airmode == Airmode::roll_pitch) { clipping_low_yaw = true; } } // check for saturation against slew rate limits if (_delta_out_max > 0.0f) { float delta_out = outputs[i] - _outputs_prev[i]; if (delta_out > _delta_out_max) { outputs[i] = _outputs_prev[i] + _delta_out_max; clipping_high = true; } else if (delta_out < -_delta_out_max) { outputs[i] = _outputs_prev[i] - _delta_out_max; clipping_low_roll_pitch = true; clipping_low_yaw = true; } } _outputs_prev[i] = outputs[i]; // update the saturation status report update_saturation_status(i, clipping_high, clipping_low_roll_pitch, clipping_low_yaw); } // this will force the caller of the mixer to always supply new slew rate values, otherwise no slew rate limiting will happen _delta_out_max = 0.0f; return _rotor_count;
}
This function starts with get_control gets the value of GROUP0, and then calls different mixers according to the Airmode
Let's see get first_ control
Mixer::get_control(uint8_t group, uint8_t index)
{
float value;
_control_cb(_cb_handle, group, index, value); return value;
}
Called_ control_cb(_cb_handle, group, index, value);
Tune in and have a look
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 */ if (input > 1.0f) { input = 1.0f; } else if (input < -1.0f) { input = -1.0f; } /* 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) { /* 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) { /* set the throttle to an invalid value */ input = NAN; } } return 0;
}
It calls input = output - >_ controls[control_ group].control[control_ index];
Assign a value to the input, which is the corresponding control_ The subscript of a group group is control_ The value of index. This_ The controls member is the output of the attitude control of the subscription. Specifically, subscribe in the bool mixingutput:: update() function. The relevant code is intercepted as follows
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++;
}
So far, the hybrid controller has obtained the desired input. The following is the torque distribution according to the four rotor model
mix_airmode_rpy as an example
void MultirotorMixer::mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs)
{
// Airmode for roll, pitch and yaw
// Do full mixing for (unsigned i = 0; i < _rotor_count; i++) { outputs[i] = roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale + yaw * _rotors[i].yaw_scale + thrust * _rotors[i].thrust_scale; // Thrust will be used to unsaturate if needed _tmp_array[i] = _rotors[i].thrust_scale; } minimize_saturation(_tmp_array, outputs, _saturation_status); // Unsaturate yaw (in case upper and lower bounds are exceeded) // to prioritize roll/pitch over yaw. for (unsigned i = 0; i < _rotor_count; i++) { _tmp_array[i] = _rotors[i].yaw_scale; } minimize_saturation(_tmp_array, outputs, _saturation_status);
}
First, the moment is distributed in a cycle. The specific method is to multiply each moment by the corresponding scaling coefficient. Take roll for example, the coefficient of roll is_ rotors[i].roll_scaleļ¼_ Rotors is a pointer to a structure
const Rotor *_rotors;
*/
struct Rotor {
float roll_scale; /< scales roll for this rotor */
float pitch_scale; /< scales pitch for this rotor */
float yaw_scale; /< scales yaw for this rotor */
float thrust_scale; /< scales thrust for this rotor */
};
Each structure has the coefficients of torque and thrust. Each such structure can realize the torque distribution of a motor. If it is a four rotor structure, it needs an array of four such structures.
The array in px4 is as follows:
const MultirotorMixer::Rotor _config_quad_x[] = {
{ -0.707107, 0.707107, 1.000000, 1.000000 },
{ 0.707107, -0.707107, 1.000000, 1.000000 },
{ 0.707107, 0.707107, -1.000000, 1.000000 },
{ -0.707107, -0.707107, -1.000000, 1.000000 },
};
According to this array, we can see that roll is the deceleration of motor 1 and motor 4, and the acceleration of motor 2 and motor 3, which is consistent with the four rotors of type X, and the distribution of other forces is the same. Four motors can be allocated by for cycle, and then the motor can be controlled by output pwm