Fix error in rc expo calculation // add configurable expo power

src/main/common/maths.c

@@ -99,6 +99,13 @@ float acos_approx(float x)
 }
 #endif
 
+float powerf(float base, int exp) {
+    float result = base;
+    for (int count = 1; count < exp; count++) result *= base;
+
+    return result;
+}
+
 int32_t applyDeadband(int32_t value, int32_t deadband)
 {
     if (ABS(value) < deadband) {

src/main/common/maths.h

@@ -69,6 +69,7 @@ typedef union {
     fp_angles_def angles;
 } fp_angles_t;
 
+float powerf(float base, int exp);
 int32_t applyDeadband(int32_t value, int32_t deadband);
 
 void devClear(stdev_t *dev);

src/main/config/config.c

@@ -191,6 +191,7 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig)
     controlRateConfig->dynThrPID = 10;
     controlRateConfig->rcYawExpo8 = 0;
     controlRateConfig->tpa_breakpoint = 1650;
+    controlRateConfig->rcExpoPwr = 3;
 
     for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) {
         controlRateConfig->rates[axis] = 70;

src/main/io/rc_controls.h

@@ -152,6 +152,7 @@ typedef struct controlRateConfig_s {
     uint8_t dynThrPID;
     uint8_t rcYawExpo8;
     uint16_t tpa_breakpoint;                // Breakpoint where TPA is activated
+    uint8_t rcExpoPwr;
 } controlRateConfig_t;
 
 extern int16_t rcCommand[4];

src/main/io/serial_cli.c

@@ -800,6 +800,7 @@ const clivalue_t valueTable[] = {
     { "rc_rate_yaw",                VAR_UINT8  | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0,  255 } },
     { "rc_expo",                    VAR_UINT8  | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpo8, .config.minmax = { 0,  100 } },
     { "rc_yaw_expo",                VAR_UINT8  | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0,  100 } },
+    { "rc_expo_power",              VAR_UINT8  | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 2,  4 } },
     { "thr_mid",                    VAR_UINT8  | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0,  100 } },
     { "thr_expo",                   VAR_UINT8  | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0,  100 } },
     { "roll_srate",                 VAR_UINT8  | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0,  CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } },

src/main/io/serial_msp.c

@@ -820,7 +820,7 @@ static bool processOutCommand(uint8_t cmdMSP)
         serialize16((uint16_t)gyro.targetLooptime);
         break;
     case MSP_RC_TUNING:
-        headSerialReply(12);
+        headSerialReply(13);
         serialize8(currentControlRateProfile->rcRate8);
         serialize8(currentControlRateProfile->rcExpo8);
         for (i = 0 ; i < 3; i++) {
@@ -832,6 +832,7 @@ static bool processOutCommand(uint8_t cmdMSP)
         serialize16(currentControlRateProfile->tpa_breakpoint);
         serialize8(currentControlRateProfile->rcYawExpo8);
         serialize8(currentControlRateProfile->rcYawRate8);
+        serialize8(currentControlRateProfile->rcExpoPwr);
         break;
     case MSP_PID:
         headSerialReply(3 * PID_ITEM_COUNT);
@@ -1424,6 +1425,9 @@ static bool processInCommand(void)
             if (currentPort->dataSize >= 12) {
                 currentControlRateProfile->rcYawRate8 = read8();
             }
+            if (currentPort->dataSize >=13) {
+                currentControlRateProfile->rcExpoPwr = read8();
+            }
         } else {
             headSerialError(0);
         }

src/main/mw.c

@@ -189,13 +189,15 @@ float calculateSetpointRate(int axis, int16_t rc) {
 
     if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f));
     rcCommandf = rc / 500.0f;
-    rcInput[axis] = ABS(rcCommandf);
 
     if (rcExpo) {
         float expof = rcExpo / 100.0f;
-        rcCommandf = rcCommandf * (expof * (rcInput[axis] * rcInput[axis] * rcInput[axis]) + rcInput[axis]*(1-expof));
+        float absRc = ABS(rcCommandf);
+        rcCommandf = rcCommandf * (expof * (powerf(absRc, currentControlRateProfile->rcExpoPwr)) + absRc*(1-expof));
     }
 
+    rcInput[axis] = ABS(rcCommandf);
+
     angleRate = 200.0f * rcRate * rcCommandf;
 
     if (currentControlRateProfile->rates[axis]) {