Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 11:23:06 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

Multicopter mixer - Simplify and correct mistakes of roll-pitch motor saturation handling

Browse Source
This commit is contained in:
bresch
2017-09-05 11:26:06 +02:00
committed by Roman Bapst
parent 450e7c6774
commit 803eb9ac32
1 changed files with 16 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mixer/mixer_multirotor.cpp
+16 -15
View File
@@ -199,31 +199,32 @@ MultirotorMixer::mix(float *outputs, unsigned space)
float boost = 0.0f; // value added to demanded thrust (can also be negative)
float roll_pitch_scale = 1.0f; // scale for demanded roll and pitch
float delta_out_max = max_out - min_out; // distance between the two extrema
// If the difference between the to extrema is smaller than 1.0, the boost can safely unsaturate a motor if needed
// without saturating another one.
// Otherwise, a scaler is computed to make the distance between the two extrema exacly 1.0 and the boost
// value is computed to maximize the roll-pitch control.
//
// Note: thrust boost is computed assuming thrust_gain==1 for all motors.
// On asymmetric platforms, some motors have thrust_gain<1,
// which may result in motor saturation after thrust boost is applied
// TODO: revise the saturation/boosting strategy
if (delta_out_max <= 1.0f) {
if (min_out < 0.0f) {
boost = -min_out;
if (min_out < 0.0f && max_out < 1.0f && max_out - min_out <= 1.0f) {
boost = -min_out;
} else if (max_out > 1.0f) {
boost = -(max_out - 1.0f);
} else if (max_out > 1.0f && min_out > 0.0f && max_out - min_out <= 1.0f) {
boost = -(max_out - 1.0f);
}
} else if (min_out < 0.0f && max_out < 1.0f && max_out - min_out > 1.0f) {
boost = -min_out - (1.0f - max_out) / 2.0f;
roll_pitch_scale = (thrust + boost) / (thrust - min_out);
} else if (max_out > 1.0f && min_out > 0.0f && max_out - min_out > 1.0f) {
boost = -(max_out - min_out - 1.0f) / 2.0f;
roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust);
} else if (min_out < 0.0f && max_out > 1.0f) {
boost = -(max_out - 1.0f + min_out) / 2.0f;
roll_pitch_scale = (thrust + boost) / (thrust - min_out);
} else {
roll_pitch_scale = 1 / (max_out - min_out);
boost = 1.0f - ((max_out - thrust) * roll_pitch_scale + thrust);
}
// capture saturation
if (min_out < 0.0f) {
_saturation_status.flags.motor_neg = true;