From 1b4ac37e6665242fe078b027376e50663a9883e8 Mon Sep 17 00:00:00 2001
From: rmackay9 <rmackay9@yahoo.com>
Date: Sat, 7 Apr 2012 12:14:25 +0900
Subject: [PATCH] AP_MotorsMatrix - fixed stability patch issue in which it
 would not limit a motor's output unless an opposite motor had been defined. 
 This would only have affected Y6 frames.

---
 libraries/AP_Motors/AP_MotorsMatrix.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp
index 36d52f2da..93944d1e3 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<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
-		if( motor_enabled[i] && opposite_motor[i] != AP_MOTORS_MATRIX_MOTOR_UNDEFINED && motor_out[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;
 		}
 	}
-- 
GitLab