fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-04 01:51:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Multirotor mixer: more careful limiting

Browse Source
This commit is contained in:
Anton Babushkin
2014-05-14 13:27:53 +02:00
parent 178a3e8567
commit b60964eb9c
1 changed files with 33 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/systemlib/mixer/mixer_multirotor.cpp
+33 -37
View File
@@ -282,56 +282,52 @@ MultirotorMixer::mix(float *outputs, unsigned space)
float yaw = get_control(0, 2) * _yaw_scale; float yaw = get_control(0, 2) * _yaw_scale;
float thrust = get_control(0, 3); float thrust = get_control(0, 3);
//lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3))); //lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3)));
float max = 0.0f; float min_out = 0.0f;
float max_out = 0.0f;
float fixup_scale; float fixup_scale;
/* use an output factor to prevent too strong control signals at low throttle */ /* perform initial mix pass yielding unbounded outputs */
float min_thrust = 0.05f;
float max_thrust = 1.0f;
float startpoint_full_control = 0.40f;
float output_factor;
/* keep roll, pitch and yaw control to 0 below min thrust */
if (thrust <= min_thrust) {
output_factor = 0.0f;
/* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */
} else if (thrust < startpoint_full_control && thrust > min_thrust) {
output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust);
/* and then stay at full control */
} else {
output_factor = max_thrust;
}
roll *= output_factor;
pitch *= output_factor;
yaw *= output_factor;
/* perform initial mix pass yielding un-bounded outputs */
for (unsigned i = 0; i < _rotor_count; i++) { for (unsigned i = 0; i < _rotor_count; i++) {
float tmp = roll * _rotors[i].roll_scale + float out = roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale + pitch * _rotors[i].pitch_scale +
yaw * _rotors[i].yaw_scale + yaw * _rotors[i].yaw_scale +
thrust; thrust;
if (tmp > max) if (out < min_out) {
max = tmp; min_out = out;
}
if (out > max_out) {
max_out = out;
}
outputs[i] = tmp; outputs[i] = out;
} }
/* scale values into the -1.0 - 1.0 range */ /* scale down controls if some outputs are negative, keep total thrust */
if (max > 1.0f) { if (min_out < 0.0) {
fixup_scale = 2.0f / max; float scale_in = thrust / (thrust - min_out);
/* mix again with adjusted controls */
for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] = scale_in * (roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale +
yaw * _rotors[i].yaw_scale) +
thrust;
}
}
/* scale down all outputs if some outputs are too large, reduce total thrust */
float scale_out;
if (max_out > 1.0f) {
scale_out = 1.0f / max_out;
} else { } else {
fixup_scale = 2.0f; scale_out = 1.0f;
} }
for (unsigned i = 0; i < _rotor_count; i++) for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] = -1.0f + (outputs[i] * fixup_scale); outputs[i] = -1.0f + (outputs[i] * 2 * scale_out);
}
/* ensure outputs are out of the deadband */ /* ensure outputs are out of the deadband */
for (unsigned i = 0; i < _rotor_count; i++) for (unsigned i = 0; i < _rotor_count; i++)