diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index e458c58080..69482262b6 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -389,3 +389,41 @@ void AP_MotorsMatrix::remove_all_motors() remove_motor(i); } } + +// normalizes the roll, pitch and yaw factors so maximum magnitude is 0.5 +void AP_MotorsMatrix::normalise_rpy_factors() +{ + float roll_fac = 0.0f; + float pitch_fac = 0.0f; + float yaw_fac = 0.0f; + + // find maximum roll, pitch and yaw factors + for (uint8_t i=0; i