From 80c0e32660558781d33e9238253653ccfa3365a1 Mon Sep 17 00:00:00 2001
From: borisbstyle <dreambb1982@gmail.com>
Date: Mon, 6 Feb 2017 23:48:01 +0100
Subject: [PATCH] Restore multiwii throttle curve

---
 src/main/fc/config.c      |  3 +++
 src/main/fc/fc_msp.c      |  2 ++
 src/main/fc/fc_rc.c       | 34 +++++++++++++++++++++++++++-------
 src/main/fc/fc_rc.h       |  1 +
 src/main/fc/rc_controls.c |  2 ++
 5 files changed, 35 insertions(+), 7 deletions(-)

diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index f8a103c5f8..31ede0bf82 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -53,6 +53,7 @@
 
 #include "fc/config.h"
 #include "fc/rc_controls.h"
+#include "fc/fc_rc.h"
 #include "fc/runtime_config.h"
 
 #include "sensors/sensors.h"
@@ -883,6 +884,8 @@ static void resetConf(void)
 
 void activateConfig(void)
 {
+    generateThrottleCurve();
+
     resetAdjustmentStates();
 
     useRcControlsConfig(
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 775b3f190e..6e7adf2027 100755
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -52,6 +52,7 @@
 #include "fc/config.h"
 #include "fc/fc_core.h"
 #include "fc/fc_msp.h"
+#include "fc/fc_rc.h"
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
@@ -1399,6 +1400,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
             if (dataSize >= 12) {
                 currentControlRateProfile->rcYawRate8 = sbufReadU8(src);
             }
+            generateThrottleCurve();
         } else {
             return MSP_RESULT_ERROR;
         }
diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c
index 5c8d68b43e..8344d65d40 100755
--- a/src/main/fc/fc_rc.c
+++ b/src/main/fc/fc_rc.c
@@ -63,6 +63,32 @@ float getThrottlePIDAttenuation(void) {
     return throttlePIDAttenuation;
 }
 
+#define THROTTLE_LOOKUP_LENGTH 12
+static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH];    // lookup table for expo & mid THROTTLE
+
+void generateThrottleCurve(void)
+{
+    uint8_t i;
+
+    for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
+        int16_t tmp = 10 * i - currentControlRateProfile->thrMid8;
+        uint8_t y = 1;
+        if (tmp > 0)
+            y = 100 - currentControlRateProfile->thrMid8;
+        if (tmp < 0)
+            y = currentControlRateProfile->thrMid8;
+        lookupThrottleRC[i] = 10 * currentControlRateProfile->thrMid8 + tmp * (100 - currentControlRateProfile->thrExpo8 + (int32_t) currentControlRateProfile->thrExpo8 * (tmp * tmp) / (y * y)) / 10;
+        lookupThrottleRC[i] = PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
+    }
+}
+
+int16_t rcLookupThrottle(int32_t tmp)
+{
+    const int32_t tmp2 = tmp / 100;
+    // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
+    return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;
+}
+
 #define SETPOINT_RATE_LIMIT 1998.0f
 #define RC_RATE_INCREMENTAL 14.54f
 
@@ -269,13 +295,7 @@ void updateRcCommands(void)
         tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck);
     }
 
-    if (currentControlRateProfile->thrExpo8) {
-        float expof = currentControlRateProfile->thrExpo8 / 100.0f;
-        float tmpf = (tmp  / (PWM_RANGE_MAX - PWM_RANGE_MIN));
-        tmp = lrintf(tmp * sq(tmpf) * expof + tmp * (1-expof));
-    }
-
-    rcCommand[THROTTLE] = tmp + (PWM_RANGE_MAX - PWM_RANGE_MIN);
+    rcLookupThrottle(tmp);
 
     if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) {
         fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
diff --git a/src/main/fc/fc_rc.h b/src/main/fc/fc_rc.h
index 94eb71b4e0..7c2f378645 100755
--- a/src/main/fc/fc_rc.h
+++ b/src/main/fc/fc_rc.h
@@ -23,3 +23,4 @@ float getRcDeflectionAbs(int axis);
 float getThrottlePIDAttenuation(void);
 void updateRcCommands(void);
 void resetYawAxis(void);
+void generateThrottleCurve(void);
diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c
index 29137dd160..0f8a79b01e 100644
--- a/src/main/fc/rc_controls.c
+++ b/src/main/fc/rc_controls.c
@@ -37,6 +37,7 @@
 #include "fc/config.h"
 #include "fc/fc_core.h"
 #include "fc/rc_controls.h"
+#include "fc/fc_rc.h"
 #include "fc/runtime_config.h"
 
 #include "io/gps.h"
@@ -522,6 +523,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
         case ADJUSTMENT_THROTTLE_EXPO:
             newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c
             controlRateConfig->thrExpo8 = newValue;
+            generateThrottleCurve();
             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue);
         break;
         case ADJUSTMENT_PITCH_ROLL_RATE: