|
|
|
@ -317,9 +317,10 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
@@ -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)
@@ -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; |
|
|
|
|