From 8f8eb7e21402fa2426d5bb578eb724133e4b95c4 Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Thu, 4 Feb 2016 20:45:49 +0900
Subject: [PATCH] AP_MotorsMatrix: add normalise_rpy_factors

---
 libraries/AP_Motors/AP_MotorsMatrix.cpp | 38 +++++++++++++++++++++++++
 libraries/AP_Motors/AP_MotorsMatrix.h   |  3 ++
 2 files changed, 41 insertions(+)

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<AP_MOTORS_MAX_NUM_MOTORS; i++) {
+        if (motor_enabled[i]) {
+            if (roll_fac < fabsf(_roll_factor[i])) {
+                roll_fac = fabsf(_roll_factor[i]);
+            }
+            if (pitch_fac < fabsf(_pitch_factor[i])) {
+                pitch_fac = fabsf(_pitch_factor[i]);
+            }
+            if (yaw_fac < fabsf(_yaw_factor[i])) {
+                yaw_fac = fabsf(_yaw_factor[i]);
+            }
+        }
+    }
+
+    // scale factors back to -0.5 to +0.5 for each axis
+    for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
+        if (motor_enabled[i]) {
+            if (!is_zero(roll_fac)) {
+                _roll_factor[i] = 0.5f*_roll_factor[i]/roll_fac;
+            }
+            if (!is_zero(pitch_fac)) {
+                _pitch_factor[i] = 0.5f*_pitch_factor[i]/pitch_fac;
+            }
+            if (!is_zero(yaw_fac)) {
+                _yaw_factor[i] = 0.5f*_yaw_factor[i]/yaw_fac;
+            }
+        }
+    }
+}
diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h
index 9cea563aa4..44a6f3eaca 100644
--- a/libraries/AP_Motors/AP_MotorsMatrix.h
+++ b/libraries/AP_Motors/AP_MotorsMatrix.h
@@ -71,6 +71,9 @@ protected:
     // add_motor using raw roll, pitch, throttle and yaw factors
     void                add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order);
 
+    // normalizes the roll, pitch and yaw factors so maximum magnitude is 0.5
+    void                normalise_rpy_factors();
+
     float               _roll_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to roll
     float               _pitch_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to pitch
     float               _yaw_factor[AP_MOTORS_MAX_NUM_MOTORS];  // each motors contribution to yaw (normally 1 or -1)