diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 8d094b5317..aec008e1e8 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -58,7 +58,6 @@ #include "rx/rx.h" #include "sensors/battery.h" - #include "sensors/gyro.h" PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0); @@ -751,8 +750,19 @@ NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation) constrainf(pidData[FD_ROLL].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING; const float scaledAxisPidPitch = constrainf(pidData[FD_PITCH].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING; + + uint16_t yawPidSumLimit = currentPidProfile->pidSumLimitYaw; + +#ifdef USE_YAW_SPIN_RECOVERY + const bool yawSpinDetected = gyroYawSpinDetected(); + if (yawSpinDetected) { + yawPidSumLimit = PIDSUM_LIMIT_MAX; // Set to the maximum limit during yaw spin recovery to prevent limiting motor authority + } +#endif // USE_YAW_SPIN_RECOVERY + float scaledAxisPidYaw = - constrainf(pidData[FD_YAW].Sum, -currentPidProfile->pidSumLimitYaw, currentPidProfile->pidSumLimitYaw) / PID_MIXER_SCALING; + constrainf(pidData[FD_YAW].Sum, -yawPidSumLimit, yawPidSumLimit) / PID_MIXER_SCALING; + if (!mixerConfig()->yaw_motors_reversed) { scaledAxisPidYaw = -scaledAxisPidYaw; } @@ -765,11 +775,11 @@ NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation) throttle = applyThrottleLimit(throttle); } - // Handle yaw spin recovery - throttle is set to zero to prevent flyaway - // and to give the mixer full authority to stop the spin #ifdef USE_YAW_SPIN_RECOVERY - if (gyroYawSpinDetected()) { - throttle = 0.0f; + // 50% throttle provides the maximum authority for yaw recovery when airmode is not active. + // When airmode is active the throttle setting doesn't impact recovery authority. + if (yawSpinDetected && !isAirmodeActive()) { + throttle = 0.5f; // } #endif // USE_YAW_SPIN_RECOVERY diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index d2e57eb631..e4ea93b4af 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -541,6 +541,10 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an const float tpaFactor = getThrottlePIDAttenuation(); const float motorMixRange = getMotorMixRange(); +#ifdef USE_YAW_SPIN_RECOVERY + const bool yawSpinActive = gyroYawSpinDetected(); +#endif + // Dynamic i component, // gradually scale back integration when above windup point const float dynCi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f) * dT * itermAccelerator; @@ -574,7 +578,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an // Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery // It's not necessary to zero the set points for R/P because the PIDs will be zeroed below #ifdef USE_YAW_SPIN_RECOVERY - if ((axis == FD_YAW) && gyroYawSpinDetected()) { + if ((axis == FD_YAW) && yawSpinActive) { currentPidSetpoint = 0.0f; } #endif // USE_YAW_SPIN_RECOVERY @@ -628,7 +632,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor; #ifdef USE_YAW_SPIN_RECOVERY - if (gyroYawSpinDetected()) { + if (yawSpinActive) { // zero PIDs on pitch and roll leaving yaw P to correct spin pidData[axis].P = 0; pidData[axis].I = 0; @@ -643,7 +647,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an pidData[FD_PITCH].Sum = pidData[FD_PITCH].P + pidData[FD_PITCH].I + pidData[FD_PITCH].D; #ifdef USE_YAW_SPIN_RECOVERY - if (gyroYawSpinDetected()) { + if (yawSpinActive) { // yaw P alone to correct spin pidData[FD_YAW].I = 0; } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 7672f906f0..6e0ea643d8 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -30,6 +30,8 @@ #define PID_SERVO_MIXER_SCALING 0.7f #define PIDSUM_LIMIT 500 #define PIDSUM_LIMIT_YAW 400 +#define PIDSUM_LIMIT_MIN 100 +#define PIDSUM_LIMIT_MAX 1000 // Scaling factors for Pids for better tunable range in configurator for betaflight pid controller. The scaling is based on legacy pid controller or previous float #define PTERM_SCALE 0.032029f diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 93c2d17914..3e780134ed 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -731,8 +731,8 @@ const clivalue_t valueTable[] = { { "iterm_rotation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_rotation) }, { "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) }, { "iterm_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermLimit) }, - { "pidsum_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) }, - { "pidsum_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimitYaw) }, + { "pidsum_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { PIDSUM_LIMIT_MIN, PIDSUM_LIMIT_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) }, + { "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) }, { "throttle_boost", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, throttle_boost) }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 2b24d178e5..791b30cea7 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -1014,8 +1014,7 @@ static void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) } else { #ifndef SIMULATOR_BUILD // check for spin on yaw axis only - const float yawSpinTriggerRate = gyroConfig()->yaw_spin_threshold; - if (abs(gyro.gyroADCf[Z]) > yawSpinTriggerRate) { + if (abs(gyro.gyroADCf[Z]) > gyroConfig()->yaw_spin_threshold) { gyroSensor->yawSpinDetected = true; gyroSensor->yawSpinTimeUs = currentTimeUs; }