From 979dabf637520ed997fdf940eaebf7b817216197 Mon Sep 17 00:00:00 2001 From: Nicola De Pasquale Date: Mon, 21 Sep 2020 12:46:49 +0200 Subject: [PATCH] added option for expo on rc command --- src/main/cli/settings.c | 1 + src/main/fc/controlrate_profile.c | 3 ++- src/main/fc/controlrate_profile.h | 1 + src/main/fc/rc.c | 18 ++++++++++++++---- 4 files changed, 18 insertions(+), 5 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index b4f2b1e2cf..0c0383bd91 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -935,6 +935,7 @@ const clivalue_t valueTable[] = { { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, thrMid8) }, { "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, thrExpo8) }, { "rates_type", VAR_UINT8 | PROFILE_RATE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RATES_TYPE }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rates_type) }, + { "quickrates_rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, quickRatesRcExpo) }, { "roll_rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 1, CONTROL_RATE_CONFIG_RC_RATES_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rcRates[FD_ROLL]) }, { "pitch_rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 1, CONTROL_RATE_CONFIG_RC_RATES_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rcRates[FD_PITCH]) }, { "yaw_rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 1, CONTROL_RATE_CONFIG_RC_RATES_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rcRates[FD_YAW]) }, diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index b01d0eea85..d966920b04 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -37,7 +37,7 @@ controlRateConfig_t *currentControlRateProfile; -PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 2); +PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 3); void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig) { @@ -64,6 +64,7 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig) .rate_limit[FD_YAW] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX, .tpaMode = TPA_MODE_D, .profileName = { 0 }, + .quickRatesRcExpo = 0, ); } } diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index e4d95262e7..8755317d76 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -60,6 +60,7 @@ typedef struct controlRateConfig_s { uint16_t rate_limit[3]; // Sets the maximum rate for the axes uint8_t tpaMode; // Controls which PID terms TPA effects char profileName[MAX_RATE_PROFILE_NAME_LENGTH + 1]; // Descriptive name for rate profile + uint8_t quickRatesRcExpo; // Sets expo on rc command for quick rates } controlRateConfig_t; PG_DECLARE_ARRAY(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 0b60877202..09acb6e1cc 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -210,12 +210,22 @@ float applyQuickRates(const int axis, float rcCommandf, const float rcCommandfAb { const uint16_t rcRate = currentControlRateProfile->rcRates[axis] * 2; const uint16_t maxDPS = MAX(currentControlRateProfile->rates[axis] * 10, rcRate); - const float linearity = currentControlRateProfile->rcExpo[axis] / 100.0f; + const float expof = currentControlRateProfile->rcExpo[axis] / 100.0f; const float superFactorConfig = ((float)maxDPS / rcRate - 1) / ((float)maxDPS / rcRate); - float curve = power3(rcCommandfAbs) * linearity + rcCommandfAbs * (1 - linearity); - float superfactor = 1.0f / (constrainf(1.0f - (curve * superFactorConfig), 0.01f, 1.00f)); - float angleRate = constrainf(rcCommandf * rcRate * superfactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); + float curve; + float superFactor; + float angleRate; + + if (currentControlRateProfile->quickRatesRcExpo) { + curve = power3(rcCommandf) * expof + rcCommandf * (1 - expof); + superFactor = 1.0f / (constrainf(1.0f - (rcCommandfAbs * superFactorConfig), 0.01f, 1.00f)); + angleRate = constrainf(curve * rcRate * superFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); + } else { + curve = power3(rcCommandfAbs) * expof + rcCommandfAbs * (1 - expof); + superFactor = 1.0f / (constrainf(1.0f - (curve * superFactorConfig), 0.01f, 1.00f)); + angleRate = constrainf(rcCommandf * rcRate * superFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); + } return angleRate; }