From 754982f48090c1510f49c2e7560b8ccb5d896fba Mon Sep 17 00:00:00 2001
From: borisbstyle <dreambb1982@gmail.com>
Date: Mon, 30 May 2016 13:02:41 +0200
Subject: [PATCH] Add higher power function to Super Expo

---
 q                     | 59 +++++++++++++++++++++++++++++++++++++++++++
 src/main/flight/pid.c |  4 ++-
 2 files changed, 62 insertions(+), 1 deletion(-)
 create mode 100644 q

diff --git a/q b/q
new file mode 100644
index 000000000..04586033b
--- /dev/null
+++ b/q
@@ -0,0 +1,59 @@
+diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c
+index fdde2cf..53464ef 100644
+--- a/src/main/io/rc_controls.c
++++ b/src/main/io/rc_controls.c
+@@ -67,6 +67,7 @@ static pidProfile_t *pidProfile;
+ static bool isUsingSticksToArm = true;
+ 
+ int16_t rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
++int16_t rcCommandSmooth[4];     // Smoothed RcCommand
+ 
+ uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e
+ 
+diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h
+index eaec277..dd8afaf 100644
+--- a/src/main/io/rc_controls.h
++++ b/src/main/io/rc_controls.h
+@@ -147,6 +147,7 @@ typedef struct controlRateConfig_s {
+ } controlRateConfig_t;
+ 
+ extern int16_t rcCommand[4];
++extern int16_t rcCommandSmooth[4];     // Smoothed RcCommand
+ 
+ typedef struct rcControlsConfig_s {
+     uint8_t deadband;                       // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.
+diff --git a/src/main/mw.c b/src/main/mw.c
+index 125674c..5da79cf 100644
+--- a/src/main/mw.c
++++ b/src/main/mw.c
+@@ -181,7 +181,7 @@ void filterRc(void)
+ 
+     if (isRXDataNew) {
+         for (int channel=0; channel < 4; channel++) {
+-            deltaRC[channel] = rcCommand[channel] -  (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
++            deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
+             lastCommand[channel] = rcCommand[channel];
+         }
+ 
+@@ -194,7 +194,7 @@ void filterRc(void)
+     // Interpolate steps of rcCommand
+     if (factor > 0) {
+         for (int channel=0; channel < 4; channel++) {
+-            rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
++            rcCommandSmooth[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
+          }
+     } else {
+         factor = 0;
+@@ -649,8 +649,11 @@ void subTaskMainSubprocesses(void) {
+ 
+     const uint32_t startTime = micros();
+ 
++    filterRc();
++
+     if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) {
+-        filterRc();
++        int axis;
++        for (axis = 0; axis <= 4; axis++) rcCommand[axis] = rcCommandSmooth[axis];
+     }
+ 
+     // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 5983e7a29..11fb52abb 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -81,8 +81,10 @@ float calculateExpoPlus(int axis, const rxConfig_t *rxConfig) {
     if (axis == YAW && !rxConfig->superExpoYawMode) {
         propFactor = 1.0f;
     } else {
+        float rcFactor = (ABS(rcCommand[axis]) / 500.0f);
+
         superExpoFactor = (axis == YAW) ? rxConfig->superExpoFactorYaw : rxConfig->superExpoFactor;
-        propFactor = constrainf(1.0f - ((superExpoFactor / 100.0f) * (ABS(rcCommand[axis]) / 500.0f)), 0.0f, 1.0f);
+        propFactor = constrainf(1.0f - ((superExpoFactor / 100.0f) * rcFactor * rcFactor * rcFactor), 0.0f, 1.0f);
     }
 
     return propFactor;