From d9922c8227afbef5d880197931a0161f59d0f0d3 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sat, 20 Jan 2018 16:16:45 +1300 Subject: [PATCH] Fixed Betaflight rates calculation. Moved `rates_type` into rate profile config in CLI. --- src/main/fc/fc_rc.c | 10 ++++++---- src/main/interface/settings.c | 2 +- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index f108eed5e8..3b2e69ae26 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -46,7 +46,7 @@ #include "sensors/battery.h" -typedef float (applyRatesFn)(int axis, float rcCommandf, const float rcCommandfAbs); +typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCommandfAbs); static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; static float throttlePIDAttenuation; @@ -86,14 +86,15 @@ static int16_t rcLookupThrottle(int32_t tmp) #define SETPOINT_RATE_LIMIT 1998.0f #define RC_RATE_INCREMENTAL 14.54f -float applyBetaflightRates(int axis, float rcCommandf, const float rcCommandfAbs) +float applyBetaflightRates(const int axis, float rcCommandf, const float rcCommandfAbs) { if (currentControlRateProfile->rcExpo[axis]) { const float expof = currentControlRateProfile->rcExpo[axis] / 100.0f; rcCommandf = rcCommandf * power3(rcCommandfAbs) * expof + rcCommandf * (1 - expof); } - float angleRate = 200.0f * currentControlRateProfile->rcRates[axis] * rcCommandf; + const float rcRate = currentControlRateProfile->rcRates[axis] / 100.0f; + float angleRate = 200.0f * rcRate * rcCommandf; if (currentControlRateProfile->rates[axis]) { const float rcSuperfactor = 1.0f / (constrainf(1.0f - (rcCommandfAbs * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); angleRate *= rcSuperfactor; @@ -102,7 +103,7 @@ float applyBetaflightRates(int axis, float rcCommandf, const float rcCommandfAbs return angleRate; } -float applyRaceFlightRates(int axis, float rcCommandf, const float rcCommandfAbs) +float applyRaceFlightRates(const int axis, float rcCommandf, const float rcCommandfAbs) { UNUSED(rcCommandfAbs); @@ -387,6 +388,7 @@ void initRcProcessing(void) switch (currentControlRateProfile->rates_type) { case RATES_TYPE_BETAFLIGHT: + default: applyRates = applyBetaflightRates; break; diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 014a1e24ab..9bdbf63946 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -556,7 +556,7 @@ const clivalue_t valueTable[] = { // PG_CONTROLRATE_PROFILES { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmax = { 0, 100 }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, thrMid8) }, { "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmax = { 0, 100 }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, thrExpo8) }, - { "rates_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RATES_TYPE }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rates_type) }, + { "rates_type", VAR_UINT8 | PROFILE_RATE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RATES_TYPE }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rates_type) }, { "roll_rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmax = { 0, 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.minmax = { 0, 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.minmax = { 0, CONTROL_RATE_CONFIG_RC_RATES_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rcRates[FD_YAW]) },