From cfad556a1609048029e66f540c7e87e87f2d8905 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 11 Dec 2018 18:47:33 +0100 Subject: [PATCH] mixer_multirotor: use reduce_only instead of -1000 for yaw thrust reduction This is more accurate, because if we use -1000 as lower bound, a very large yaw command can reduce thrust more than needed. The difference can be seen in the following example (with roll/pitch airmode): p_dot_sp = -1.0 # roll acceleration q_dot_sp = 0.9 # pitch acceleration r_dot_sp = -0.9 # yaw acceleration --- src/lib/mixer/mixer_multirotor.cpp | 3 ++- src/lib/mixer/mixer_multirotor.py | 5 ++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp index d7bdd3d0b5..adb3702bb0 100644 --- a/src/lib/mixer/mixer_multirotor.cpp +++ b/src/lib/mixer/mixer_multirotor.cpp @@ -339,7 +339,8 @@ void MultirotorMixer::mix_yaw(float yaw, float *outputs) _tmp_array[i] = _rotors[i].thrust_scale; } - minimize_saturation(_tmp_array, outputs, _saturation_status, -1000.f); + // reduce thrust only + minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.f, true); } unsigned diff --git a/src/lib/mixer/mixer_multirotor.py b/src/lib/mixer/mixer_multirotor.py index 5420c82b3d..b1a5b158d0 100755 --- a/src/lib/mixer/mixer_multirotor.py +++ b/src/lib/mixer/mixer_multirotor.py @@ -92,7 +92,10 @@ def mix_yaw(m_sp, u, P, u_min, u_max): u_r_dot = P[:,2] u_pp = minimize_sat(u_p, u_min, u_max+0.15, u_r_dot) u_T = P[:, 3] - u_ppp = minimize_sat(u_pp, -1000, u_max, u_T) + u_ppp = minimize_sat(u_pp, 0, u_max, u_T) + # reduce thrust only + if (u_ppp > (u_pp)).any(): + u_ppp = u_pp return u_ppp def airmode_rp(m_sp, P, u_min, u_max):