diff options
author | Lorenz Meier <lorenz@px4.io> | 2015-03-21 23:15:33 -0700 |
---|---|---|
committer | Lorenz Meier <lorenz@px4.io> | 2015-03-21 23:15:33 -0700 |
commit | 07389eabbe1c00dbcb7141249b932399195af43a (patch) | |
tree | e0eb2074f58abc56f6d8c6463683df9b096c8d4f /src/modules | |
parent | 49834b11cbea6fd0bb69650573cc275f01b0dbc5 (diff) | |
parent | 95be7c22896571ca7555ee54eae7fac43cc867a4 (diff) | |
download | px4-firmware-07389eabbe1c00dbcb7141249b932399195af43a.tar.gz px4-firmware-07389eabbe1c00dbcb7141249b932399195af43a.tar.bz2 px4-firmware-07389eabbe1c00dbcb7141249b932399195af43a.zip |
Merge pull request #1942 from PX4/mc_mixer_limit
Fix limiting in mc mixer
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/systemlib/mixer/mixer_multirotor.cpp | 13 |
1 files changed, 11 insertions, 2 deletions
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index b354eb518..b753da5ce 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -244,14 +244,23 @@ MultirotorMixer::mix(float *outputs, unsigned space) if (min_out < 0.0f) { float scale_in = thrust / (thrust - min_out); + max_out = 0.0f; + /* 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) + thrust; + float out = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust; + + /* update max output value */ + if (out > max_out) { + max_out = out; + } + + outputs[i] = out; } _limits.roll_pitch = true; } else { - /* roll/pitch mixed without limiting, add yaw control */ + /* roll/pitch mixed without lower side limiting, add yaw control */ for (unsigned i = 0; i < _rotor_count; i++) { outputs[i] += yaw * _rotors[i].yaw_scale; } |