From 2bd92b5a4d116a48b3ee16cc907d8dc08d8dd798 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 5 Jun 2019 15:08:28 +0200 Subject: [PATCH] mc mixer: prioritize roll/pitch over yaw for full airmode Improves roll/pitch tracking in situations of large yaw demands. --- src/lib/mixer/mixer.h | 3 ++- src/lib/mixer/mixer_multirotor.cpp | 8 ++++++++ src/lib/mixer/mixer_multirotor.py | 8 +++++++- 3 files changed, 17 insertions(+), 2 deletions(-) diff --git a/src/lib/mixer/mixer.h b/src/lib/mixer/mixer.h index 433b07c890ff..99113ad5b371 100644 --- a/src/lib/mixer/mixer.h +++ b/src/lib/mixer/mixer.h @@ -773,7 +773,8 @@ class MultirotorMixer : public Mixer * Mix roll, pitch, yaw, thrust and set the outputs vector. * * Desaturation behavior: full airmode for roll/pitch/yaw: - * thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw. + * thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw, + * while giving priority to roll and pitch over yaw. */ inline void mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs); diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp index 72ea8bd4d22d..ce04811d9e5c 100644 --- a/src/lib/mixer/mixer_multirotor.cpp +++ b/src/lib/mixer/mixer_multirotor.cpp @@ -285,6 +285,14 @@ void MultirotorMixer::mix_airmode_rpy(float roll, float pitch, float yaw, float } 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); } void MultirotorMixer::mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs) diff --git a/src/lib/mixer/mixer_multirotor.py b/src/lib/mixer/mixer_multirotor.py index 7d735ea5af0c..2e80049d3841 100755 --- a/src/lib/mixer/mixer_multirotor.py +++ b/src/lib/mixer/mixer_multirotor.py @@ -134,7 +134,13 @@ def airmode_rpy(m_sp, P, u_min, u_max): # Use thrust to unsaturate the outputs if needed u_T = P[:, 3] u_prime = minimize_sat(u, u_min, u_max, u_T) - return (u, u_prime) + + # Unsaturate yaw (in case upper and lower bounds are exceeded) + # to prioritize roll/pitch over yaw. + u_T = P[:, 2] + u_prime_yaw = minimize_sat(u_prime, u_min, u_max, u_T) + + return (u, u_prime_yaw) def normal_mode(m_sp, P, u_min, u_max):