From 751dbb7df75ed2964c22600bcaf3c0f2f8ba4ee1 Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Sat, 21 Feb 2015 16:55:13 +0900
Subject: [PATCH] Motors: move thrust curve and volt scaling to parent

Moving from MotorsMatrix to Motors allows it to be used from other frames
---
 libraries/AP_Motors/AP_MotorsMatrix.cpp | 15 +++++----------
 libraries/AP_Motors/AP_Motors_Class.cpp | 10 ++++++++++
 libraries/AP_Motors/AP_Motors_Class.h   |  3 +++
 3 files changed, 18 insertions(+), 10 deletions(-)

diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp
index e4cde1ce25..b31ffba83b 100644
--- a/libraries/AP_Motors/AP_MotorsMatrix.cpp
+++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp
@@ -352,18 +352,13 @@ void AP_MotorsMatrix::output_armed()
             }
         }
 
-        // adjust for throttle curve
-        if (_throttle_curve_enabled) {
-            for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
-                if (motor_enabled[i]) {
-                    float temp_out = ((float)(motor_out[i]-out_min_pwm))/((float)(out_max_pwm-out_min_pwm));
-                    if (_thrust_curve_expo > 0.0f){
-                        temp_out = ((_thrust_curve_expo-1.0f) + safe_sqrt((1.0f-_thrust_curve_expo)*(1.0f-_thrust_curve_expo) + 4.0f*_thrust_curve_expo*_lift_max*temp_out))/(2.0f*_thrust_curve_expo*_batt_voltage_filt);
-                    }
-                    motor_out[i] = temp_out*(_thrust_curve_max*out_max_pwm-out_min_pwm)+out_min_pwm;
-                }
+        // apply thrust curve and voltage scaling
+        for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
+            if (motor_enabled[i]) {
+                motor_out[i] = apply_thrust_curve_and_volt_scaling(motor_out[i], out_min_pwm, out_max_pwm);
             }
         }
+
         // clip motor output if required (shouldn't be)
         for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
             if (motor_enabled[i]) {
diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp
index 10b664339f..613a0a1b18 100644
--- a/libraries/AP_Motors/AP_Motors_Class.cpp
+++ b/libraries/AP_Motors/AP_Motors_Class.cpp
@@ -284,3 +284,13 @@ void AP_Motors::update_max_throttle()
         _flags.slow_start = false;
     }
 }
+
+// apply_thrust_curve_and_volt_scaling - returns throttle curve adjusted pwm value (i.e. 1000 ~ 2000)
+int16_t AP_Motors::apply_thrust_curve_and_volt_scaling(int16_t pwm_out, int16_t pwm_min, int16_t pwm_max) const
+{
+    float temp_out = ((float)(pwm_out-pwm_min))/((float)(pwm_max-pwm_min));
+    if (_thrust_curve_expo > 0.0f){
+        temp_out = ((_thrust_curve_expo-1.0f) + safe_sqrt((1.0f-_thrust_curve_expo)*(1.0f-_thrust_curve_expo) + 4.0f*_thrust_curve_expo*_lift_max*temp_out))/(2.0f*_thrust_curve_expo*_batt_voltage_filt);
+    }
+    return (temp_out*(_thrust_curve_max*pwm_max-pwm_min)+pwm_min);
+}
diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h
index b66e7d9d93..6de0a93596 100644
--- a/libraries/AP_Motors/AP_Motors_Class.h
+++ b/libraries/AP_Motors/AP_Motors_Class.h
@@ -180,6 +180,9 @@ protected:
     // update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag
     void                update_max_throttle();
 
+    // apply_thrust_curve_and_volt_scaling - thrust curve and voltage adjusted pwm value (i.e. 1000 ~ 2000)
+    int16_t             apply_thrust_curve_and_volt_scaling(int16_t pwm_out, int16_t pwm_min, int16_t pwm_max) const;
+
     // flag bitmask
     struct AP_Motors_flags {
         uint8_t armed               : 1;    // 1 if the motors are armed, 0 if disarmed