diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 52990c6ac8..ee1b65ce67 100644 --- a/src/main/common/maths.c +++ b/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) { diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 3251c237a8..3b24dd734d 100644 --- a/src/main/common/maths.h +++ b/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); diff --git a/src/main/config/config.c b/src/main/config/config.c index f5aabf3629..69b221b000 100755 --- a/src/main/config/config.c +++ b/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; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 38c093f537..55e3cecbaf 100644 --- a/src/main/io/rc_controls.h +++ b/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]; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 9509d0caa2..8a163f6ce3 100644 --- a/src/main/io/serial_cli.c +++ b/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 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 52de0e4ed1..1e1c9bed4c 100644 --- a/src/main/io/serial_msp.c +++ b/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); } diff --git a/src/main/mw.c b/src/main/mw.c index 08bf53420c..01322d98ca 100644 --- a/src/main/mw.c +++ b/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]) {