Yaw spin recovery optimize (#3)

* PID controller unittest

* Clean code for yaw spin recovery

* Yaw spin recovery optimizations

* Flash size optimizations, use 50% throttle when airmode is off, and override pidsum_limit_yaw

Also rebasing from betaflight/master

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()) {
+    // 50% throttle provides the maximum authority for yaw recovery when airmode is not active.
-        throttle = 0.0f;
+    // When airmode is active the throttle setting doesn't impact recovery authority.
+    if (yawSpinDetected && !isAirmodeActive()) {
+        throttle = 0.5f;   // 
     }
 #endif // USE_YAW_SPIN_RECOVERY
 

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;
     }

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

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",               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 = { 100, 1000 }, 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) },
 
     { "throttle_boost",             VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, throttle_boost) },

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]) > gyroConfig()->yaw_spin_threshold) {
-         if (abs(gyro.gyroADCf[Z]) > yawSpinTriggerRate) {
             gyroSensor->yawSpinDetected = true;
             gyroSensor->yawSpinTimeUs = currentTimeUs;
         }