diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 36d52f2da1..93944d1e37 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -146,14 +146,14 @@ void AP_MotorsMatrix::output_armed() _rc_pitch->pwm_out * _pitch_factor[i] + _rc_yaw->pwm_out*_yaw_factor[i]; } - // ensure motor is not below the minimum - motor_out[AP_MOTORS_MOT_1] = max(motor_out[AP_MOTORS_MOT_1], out_min); } // stability patch for( i=0; i out_max ) { - motor_out[opposite_motor[i]] -= motor_out[i] - out_max; + if( motor_enabled[i] && motor_out[i] > out_max ) { + if( opposite_motor[i] != AP_MOTORS_MATRIX_MOTOR_UNDEFINED ) { + motor_out[opposite_motor[i]] -= motor_out[i] - out_max; + } motor_out[i] = out_max; } }