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