From 262d9c790bf0d3cd65263472236e9a8a9714723f Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Tue, 9 Jan 2018 08:10:36 +0100 Subject: [PATCH] MultirotorMixer: Apply thrust gain independently from other axes Align the way motor output is computed with the model used in the mixer generation script. previous: `out = (roll_gain*roll + pitch_gain*pitch + yaw_gain*yaw + thrust)*out_gain` new: `out = roll_gain*roll + pitch_gain*pitch + yaw_gain*yaw + thrust_gain*thrust` which is the standard matrix*vector multiplication. This commit has 0 effect on symmetric platforms (i.e. most) as thrust_gain==1 on these platforms. On asymmetric platform --where some thrust_gain are lower than 1-- the previous formulation resulted in coupling between thrust and other axes. fixes #8628 --- src/lib/mixer/mixer.h | 6 +++--- src/lib/mixer/mixer_multirotor.cpp | 15 ++++++++------- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/src/lib/mixer/mixer.h b/src/lib/mixer/mixer.h index b51de24e39..3ccf0ba867 100644 --- a/src/lib/mixer/mixer.h +++ b/src/lib/mixer/mixer.h @@ -546,10 +546,10 @@ public: * Precalculated rotor mix. */ struct Rotor { - float roll_scale; /**< scales roll for this rotor */ + float roll_scale; /**< scales roll for this rotor */ float pitch_scale; /**< scales pitch for this rotor */ - float yaw_scale; /**< scales yaw for this rotor */ - float out_scale; /**< scales total out for this rotor */ + float yaw_scale; /**< scales yaw for this rotor */ + float thrust_scale; /**< scales thrust for this rotor */ }; /** diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp index 0a34a4fba4..8bd087d9a2 100644 --- a/src/lib/mixer/mixer_multirotor.cpp +++ b/src/lib/mixer/mixer_multirotor.cpp @@ -187,9 +187,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) for (unsigned i = 0; i < _rotor_count; i++) { float out = roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale + - thrust; - - out *= _rotors[i].out_scale; + thrust * _rotors[i].thrust_scale; /* calculate min and max output values */ if (out < min_out) { @@ -206,6 +204,11 @@ 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 + // 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 (min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) { float max_thrust_diff = thrust * thrust_increase_factor - thrust; @@ -262,9 +265,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) float out = (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + yaw * _rotors[i].yaw_scale + - thrust + boost; - - out *= _rotors[i].out_scale; + (thrust + boost) * _rotors[i].thrust_scale; // scale yaw if it violates limits. inform about yaw limit reached if (out < 0.0f) { @@ -300,7 +301,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) outputs[i] = (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + yaw * _rotors[i].yaw_scale + - thrust + boost; + (thrust + boost) * _rotors[i].thrust_scale; /* implement simple model for static relationship between applied motor pwm and motor thrust