diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index d178d2f856..80b5aae0c2 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -336,7 +336,7 @@ static const char * const lookupTableGyroOverflowCheck[] = { #endif static const char * const lookupTableRatesType[] = { - "BETAFLIGHT", "RACEFLIGHT", "KISS" + "BETAFLIGHT", "RACEFLIGHT", "KISS", "ACTUAL" }; #ifdef USE_OVERCLOCK diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index 08fd20119b..16ad114392 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -28,6 +28,7 @@ typedef enum { RATES_TYPE_BETAFLIGHT = 0, RATES_TYPE_RACEFLIGHT, RATES_TYPE_KISS, + RATES_TYPE_ACTUAL, } ratesType_e; typedef enum { diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index d45729c818..74b2e8ae6c 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -193,6 +193,18 @@ float applyKissRates(const int axis, float rcCommandf, const float rcCommandfAbs return kissAngle; } +float applyActualRates(const int axis, float rcCommandf, const float rcCommandfAbs) +{ + float expof = currentControlRateProfile->rcExpo[axis] / 100.0f; + expof = rcCommandfAbs * (powerf(rcCommandf, 5) * expof + rcCommandf * (1 - expof)); + + const float centerSensitivity = currentControlRateProfile->rcRates[axis]; + const float stickMovement = MAX(0, currentControlRateProfile->rates[axis] * 10.0f - centerSensitivity); + const float angleRate = rcCommandf * centerSensitivity + stickMovement * expof; + + return angleRate; +} + float applyCurve(int axis, float deflection) { return applyRates(axis, deflection, fabsf(deflection)); @@ -816,6 +828,10 @@ void initRcProcessing(void) case RATES_TYPE_KISS: applyRates = applyKissRates; + break; + case RATES_TYPE_ACTUAL: + applyRates = applyActualRates; + break; }