diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 52de4ef6f9..38074f7ae5 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -317,9 +317,10 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) if (out < 0.0f) { if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) { yaw = 0.0f; + } else { yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * - roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale; + roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale; } if (status_reg != NULL) { @@ -330,8 +331,10 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) // allow to reduce thrust to get some yaw response float thrust_reduction = fminf(0.15f, out - 1.0f); thrust -= thrust_reduction; + if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) { yaw = 0.0f; + } else { yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + thrust + boost)) / _rotors[i].yaw_scale;