1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 15:25:36 +03:00

Added validation checks for interpolation settings, made throttle boost conditional.

This commit is contained in:
mikeller 2018-06-04 23:19:05 +12:00
parent 91013da2cc
commit a373204d7a
5 changed files with 29 additions and 2 deletions

View file

@ -266,6 +266,22 @@ static void validateAndFixConfig(void)
} }
#endif #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. // clear features that are not supported.
// I have kept them all here in one place, some could be moved to sections of code above. // I have kept them all here in one place, some could be moved to sections of code above.

View file

@ -803,12 +803,14 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
motorMix[i] = mix; motorMix[i] = mix;
} }
motorMixRange = motorMixMax - motorMixMin; #if defined(USE_THROTTLE_BOOST)
if (throttleBoost > 0.0f) { if (throttleBoost > 0.0f) {
float throttlehpf = throttle - pt1FilterApply(&throttleLpf, throttle); float throttlehpf = throttle - pt1FilterApply(&throttleLpf, throttle);
throttle = constrainf(throttle + throttleBoost * throttlehpf, 0.0f, 1.0f); throttle = constrainf(throttle + throttleBoost * throttlehpf, 0.0f, 1.0f);
} }
#endif
motorMixRange = motorMixMax - motorMixMin;
if (motorMixRange > 1.0f) { if (motorMixRange > 1.0f) {
for (int i = 0; i < motorCount; i++) { for (int i = 0; i < motorCount; i++) {
motorMix[i] /= motorMixRange; motorMix[i] /= motorMixRange;

View file

@ -294,7 +294,9 @@ void pidInitFilters(const pidProfile_t *pidProfile)
pt1FilterInit(&ptermYawLowpass, pt1FilterGain(pidProfile->yaw_lowpass_hz, dT)); pt1FilterInit(&ptermYawLowpass, pt1FilterGain(pidProfile->yaw_lowpass_hz, dT));
} }
#if defined(USE_THROTTLE_BOOST)
pt1FilterInit(&throttleLpf, pt1FilterGain(pidProfile->throttle_boost_cutoff, dT)); pt1FilterInit(&throttleLpf, pt1FilterGain(pidProfile->throttle_boost_cutoff, dT));
#endif
#if defined(USE_ITERM_RELAX) #if defined(USE_ITERM_RELAX)
if (itermRelax) { if (itermRelax) {
for (int i = 0; i < XYZ_AXIS_COUNT; i++) { 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 crashSetpointThreshold;
static FAST_RAM_ZERO_INIT float crashLimitYaw; static FAST_RAM_ZERO_INIT float crashLimitYaw;
static FAST_RAM_ZERO_INIT float itermLimit; static FAST_RAM_ZERO_INIT float itermLimit;
#if defined(USE_THROTTLE_BOOST)
FAST_RAM_ZERO_INIT float throttleBoost; FAST_RAM_ZERO_INIT float throttleBoost;
pt1Filter_t throttleLpf; pt1Filter_t throttleLpf;
#endif
static FAST_RAM_ZERO_INIT bool itermRotation; static FAST_RAM_ZERO_INIT bool itermRotation;
#if defined(USE_SMART_FEEDFORWARD) #if defined(USE_SMART_FEEDFORWARD)
@ -411,7 +415,9 @@ void pidInitConfig(const pidProfile_t *pidProfile)
crashSetpointThreshold = pidProfile->crash_setpoint_threshold; crashSetpointThreshold = pidProfile->crash_setpoint_threshold;
crashLimitYaw = pidProfile->crash_limit_yaw; crashLimitYaw = pidProfile->crash_limit_yaw;
itermLimit = pidProfile->itermLimit; itermLimit = pidProfile->itermLimit;
#if defined(USE_THROTTLE_BOOST)
throttleBoost = pidProfile->throttle_boost * 0.1f; throttleBoost = pidProfile->throttle_boost * 0.1f;
#endif
itermRotation = pidProfile->iterm_rotation; itermRotation = pidProfile->iterm_rotation;
#if defined(USE_SMART_FEEDFORWARD) #if defined(USE_SMART_FEEDFORWARD)
smartFeedforward = pidProfile->smart_feedforward; smartFeedforward = pidProfile->smart_feedforward;

View file

@ -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) }, { "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) }, { "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", 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) }, { "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 #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) }, { "acro_trainer_angle_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 80 }, PG_PID_PROFILE, offsetof(pidProfile_t, acro_trainer_angle_limit) },

View file

@ -189,6 +189,7 @@
#define USE_CRSF_CMS_TELEMETRY #define USE_CRSF_CMS_TELEMETRY
#define USE_BOARD_INFO #define USE_BOARD_INFO
#define USE_SMART_FEEDFORWARD #define USE_SMART_FEEDFORWARD
#define USE_THROTTLE_BOOST
#ifdef USE_SERIALRX_SPEKTRUM #ifdef USE_SERIALRX_SPEKTRUM
#define USE_SPEKTRUM_BIND #define USE_SPEKTRUM_BIND
@ -209,7 +210,6 @@
#define USE_GPS_UBLOX #define USE_GPS_UBLOX
#define USE_GPS_RESCUE #define USE_GPS_RESCUE
#define USE_OSD_ADJUSTMENTS #define USE_OSD_ADJUSTMENTS
#define USE_RC_SMOOTHING_FILTER
#define USE_SENSOR_NAMES #define USE_SENSOR_NAMES
#define USE_SERIALRX_JETIEXBUS #define USE_SERIALRX_JETIEXBUS
#define USE_TELEMETRY_IBUS #define USE_TELEMETRY_IBUS
@ -218,6 +218,7 @@
#define USE_TELEMETRY_MAVLINK #define USE_TELEMETRY_MAVLINK
#define USE_UNCOMMON_MIXERS #define USE_UNCOMMON_MIXERS
#define USE_SIGNATURE #define USE_SIGNATURE
#define USE_RC_SMOOTHING_FILTER
#define USE_ITERM_RELAX #define USE_ITERM_RELAX
#define USE_ABSOLUTE_CONTROL #define USE_ABSOLUTE_CONTROL
#endif #endif