From a373204d7a3fd5466acc2611772c617bcfa9277d Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 4 Jun 2018 23:19:05 +1200 Subject: [PATCH] Added validation checks for interpolation settings, made throttle boost conditional. --- src/main/fc/config.c | 16 ++++++++++++++++ src/main/flight/mixer.c | 4 +++- src/main/flight/pid.c | 6 ++++++ src/main/interface/settings.c | 2 ++ src/main/target/common_fc_pre.h | 3 ++- 5 files changed, 29 insertions(+), 2 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 8fec57715..500f5f927 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -266,6 +266,22 @@ static void validateAndFixConfig(void) } #endif + if (rxConfig()->rcInterpolation == RC_SMOOTHING_OFF || rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_T) { + for (unsigned i = 0; i < MAX_PROFILE_COUNT; i++) { + pidProfilesMutable(i)->dtermSetpointWeight = 0; + } + } + +#if defined(USE_THROTTLE_BOOST) + if (!(rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_RPYT + || rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_T + || rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_RPT)) { + for (unsigned i = 0; i < MAX_PROFILE_COUNT; i++) { + pidProfilesMutable(i)->throttle_boost = 0; + } + } +#endif + // clear features that are not supported. // I have kept them all here in one place, some could be moved to sections of code above. diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 21f187167..70b4d5841 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -803,12 +803,14 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa motorMix[i] = mix; } - motorMixRange = motorMixMax - motorMixMin; +#if defined(USE_THROTTLE_BOOST) if (throttleBoost > 0.0f) { float throttlehpf = throttle - pt1FilterApply(&throttleLpf, throttle); throttle = constrainf(throttle + throttleBoost * throttlehpf, 0.0f, 1.0f); } +#endif + motorMixRange = motorMixMax - motorMixMin; if (motorMixRange > 1.0f) { for (int i = 0; i < motorCount; i++) { motorMix[i] /= motorMixRange; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index cea1de270..4462f7db1 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -294,7 +294,9 @@ void pidInitFilters(const pidProfile_t *pidProfile) pt1FilterInit(&ptermYawLowpass, pt1FilterGain(pidProfile->yaw_lowpass_hz, dT)); } +#if defined(USE_THROTTLE_BOOST) pt1FilterInit(&throttleLpf, pt1FilterGain(pidProfile->throttle_boost_cutoff, dT)); +#endif #if defined(USE_ITERM_RELAX) if (itermRelax) { for (int i = 0; i < XYZ_AXIS_COUNT; i++) { @@ -345,8 +347,10 @@ static FAST_RAM_ZERO_INIT float crashGyroThreshold; static FAST_RAM_ZERO_INIT float crashSetpointThreshold; static FAST_RAM_ZERO_INIT float crashLimitYaw; static FAST_RAM_ZERO_INIT float itermLimit; +#if defined(USE_THROTTLE_BOOST) FAST_RAM_ZERO_INIT float throttleBoost; pt1Filter_t throttleLpf; +#endif static FAST_RAM_ZERO_INIT bool itermRotation; #if defined(USE_SMART_FEEDFORWARD) @@ -411,7 +415,9 @@ void pidInitConfig(const pidProfile_t *pidProfile) crashSetpointThreshold = pidProfile->crash_setpoint_threshold; crashLimitYaw = pidProfile->crash_limit_yaw; itermLimit = pidProfile->itermLimit; +#if defined(USE_THROTTLE_BOOST) throttleBoost = pidProfile->throttle_boost * 0.1f; +#endif itermRotation = pidProfile->iterm_rotation; #if defined(USE_SMART_FEEDFORWARD) smartFeedforward = pidProfile->smart_feedforward; diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index d1e37ca5d..29044b276 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -810,8 +810,10 @@ const clivalue_t valueTable[] = { { "pidsum_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { PIDSUM_LIMIT_MIN, PIDSUM_LIMIT_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimitYaw) }, { "yaw_lowpass_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_lowpass_hz) }, +#if defined(USE_THROTTLE_BOOST) { "throttle_boost", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, throttle_boost) }, { "throttle_boost_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 5, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, throttle_boost_cutoff) }, +#endif #ifdef USE_ACRO_TRAINER { "acro_trainer_angle_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 80 }, PG_PID_PROFILE, offsetof(pidProfile_t, acro_trainer_angle_limit) }, diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 5a83c117a..b4f86691c 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -189,6 +189,7 @@ #define USE_CRSF_CMS_TELEMETRY #define USE_BOARD_INFO #define USE_SMART_FEEDFORWARD +#define USE_THROTTLE_BOOST #ifdef USE_SERIALRX_SPEKTRUM #define USE_SPEKTRUM_BIND @@ -209,7 +210,6 @@ #define USE_GPS_UBLOX #define USE_GPS_RESCUE #define USE_OSD_ADJUSTMENTS -#define USE_RC_SMOOTHING_FILTER #define USE_SENSOR_NAMES #define USE_SERIALRX_JETIEXBUS #define USE_TELEMETRY_IBUS @@ -218,6 +218,7 @@ #define USE_TELEMETRY_MAVLINK #define USE_UNCOMMON_MIXERS #define USE_SIGNATURE +#define USE_RC_SMOOTHING_FILTER #define USE_ITERM_RELAX #define USE_ABSOLUTE_CONTROL #endif -- 2.11.4.GIT