Restore multiwii throttle curve

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(

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;
         }

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) {
+    rcLookupThrottle(tmp);
-        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);
 
     if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) {
         fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);

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);

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: