Add "auto" mode to yaw spin recovery

Changes yaw spin recovery from a simple on/off to on/off/auto. In auto mode the triggering threshold is automatically calculated based on the user's current max yaw rate.

So `yaw_spin_recovery` has the following options:
`OFF`: Feature is disabled
`ON`: Feature is enabled with a manually configured `yaw_spin_threshold`
`AUTO`: Feature is enabled and the trigger threshold is automatically calculated.

The auto calculation of the trigger threshold is:
```
MAX(yawRate * 0.25, 200)
```
The 25% of the user's rate is to provide a buffer for yaw overshoots during normal flight. Overall the threshold is constrained between 500 - 1950 to match the manual selection.

The default is changed from `ON` to `AUTO` so that users will get protection without needing to configure the threshold manually.

In `AUTO` mode the threshold is calculated dynamically so rate changes in flight (like rateprofile change) will set a new trigger threshold.

src/main/cli/settings.c

@@ -639,8 +639,8 @@ const clivalue_t valueTable[] = {
     { "gyro_overflow_detect",       VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_OVERFLOW_CHECK }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, checkOverflow) },
 #endif
 #ifdef USE_YAW_SPIN_RECOVERY
-    { "yaw_spin_recovery",          VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, yaw_spin_recovery) },
-    { "yaw_spin_threshold",         VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 500,  1950 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, yaw_spin_threshold) },
+    { "yaw_spin_recovery",          VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON_AUTO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, yaw_spin_recovery) },
+    { "yaw_spin_threshold",         VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { YAW_SPIN_RECOVERY_THRESHOLD_MIN,  YAW_SPIN_RECOVERY_THRESHOLD_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, yaw_spin_threshold) },
 #endif
 
 #ifdef USE_MULTI_GYRO

src/main/fc/rc.c

@@ -53,6 +53,7 @@
 #include "rx/rx.h"
 
 #include "sensors/battery.h"
+#include "sensors/gyro.h"
 
 #include "rc.h"
 
@@ -841,6 +842,11 @@ void initRcProcessing(void)
 
         break;
     }
+
+#ifdef USE_YAW_SPIN_RECOVERY
+    const int maxYawRate = (int)applyRates(FD_YAW, 1.0f, 1.0f);
+    initYawSpinRecovery(maxYawRate);
+#endif
 }
 
 bool rcSmoothingIsEnabled(void)

src/main/sensors/gyro.c

@@ -100,6 +100,8 @@ static FAST_RAM_ZERO_INIT uint8_t overflowAxisMask;
 #endif
 
 #ifdef USE_YAW_SPIN_RECOVERY
+static FAST_RAM_ZERO_INIT bool yawSpinRecoveryEnabled;
+static FAST_RAM_ZERO_INIT int yawSpinRecoveryThreshold;
 static FAST_RAM_ZERO_INIT bool yawSpinDetected;
 static FAST_RAM_ZERO_INIT timeUs_t yawSpinTimeUs;
 #endif
@@ -180,7 +182,7 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
     gyroConfig->gyro_soft_notch_cutoff_2 = 0;
     gyroConfig->checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES;
     gyroConfig->gyro_offset_yaw = 0;
-    gyroConfig->yaw_spin_recovery = true;
+    gyroConfig->yaw_spin_recovery = YAW_SPIN_RECOVERY_AUTO;
     gyroConfig->yaw_spin_threshold = 1950;
     gyroConfig->dyn_lpf_gyro_min_hz = 200;
     gyroConfig->dyn_lpf_gyro_max_hz = 500;
@@ -998,7 +1000,7 @@ static FAST_CODE void checkForOverflow(timeUs_t currentTimeUs)
 #ifdef USE_YAW_SPIN_RECOVERY
 static FAST_CODE_NOINLINE void handleYawSpin(timeUs_t currentTimeUs)
 {
-    const float yawSpinResetRate = gyroConfig()->yaw_spin_threshold - 100.0f;
+    const float yawSpinResetRate = yawSpinRecoveryThreshold - 100.0f;
     if (fabsf(gyro.gyroADCf[Z]) < yawSpinResetRate) {
         // testing whether 20ms of consecutive OK gyro yaw values is enough
         if (cmpTimeUs(currentTimeUs, yawSpinTimeUs) > 20000) {
@@ -1025,7 +1027,7 @@ static FAST_CODE void checkForYawSpin(timeUs_t currentTimeUs)
     } else {
 #ifndef SIMULATOR_BUILD
         // check for spin on yaw axis only
-         if (abs((int)gyro.gyroADCf[Z]) > gyroConfig()->yaw_spin_threshold) {
+         if (abs((int)gyro.gyroADCf[Z]) > yawSpinRecoveryThreshold) {
             yawSpinDetected = true;
             yawSpinTimeUs = currentTimeUs;
         }
@@ -1181,7 +1183,7 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
 #endif
 
 #ifdef USE_YAW_SPIN_RECOVERY
-    if (gyroConfig()->yaw_spin_recovery) {
+    if (yawSpinRecoveryEnabled) {
         checkForYawSpin(currentTimeUs);
     }
 #endif
@@ -1310,3 +1312,30 @@ void dynLpfGyroUpdate(float throttle)
     }
 }
 #endif
+
+#ifdef USE_YAW_SPIN_RECOVERY
+void initYawSpinRecovery(int maxYawRate)
+{
+    bool enabledFlag;
+    int threshold;
+
+    switch (gyroConfig()->yaw_spin_recovery) {
+    case YAW_SPIN_RECOVERY_OFF:
+        enabledFlag = false;
+        threshold = YAW_SPIN_RECOVERY_THRESHOLD_MAX;
+        break;
+    case YAW_SPIN_RECOVERY_ON:
+        enabledFlag = true;
+        threshold = gyroConfig()->yaw_spin_threshold;
+        break;
+    case YAW_SPIN_RECOVERY_AUTO:
+        enabledFlag = true;
+        const int overshootAllowance = MAX(maxYawRate / 4, 200); // Allow a 25% or minimum 200dps overshoot tolerance
+        threshold = constrain(maxYawRate + overshootAllowance, YAW_SPIN_RECOVERY_THRESHOLD_MIN, YAW_SPIN_RECOVERY_THRESHOLD_MAX);
+        break;
+    }
+
+    yawSpinRecoveryEnabled = enabledFlag;
+    yawSpinRecoveryThreshold = threshold;
+}
+#endif

src/main/sensors/gyro.h

@@ -38,6 +38,11 @@
 
 #define FILTER_FREQUENCY_MAX 4000 // maximum frequency for filter cutoffs (nyquist limit of 8K max sampling)
 
+#ifdef USE_YAW_SPIN_RECOVERY
+#define YAW_SPIN_RECOVERY_THRESHOLD_MIN 500
+#define YAW_SPIN_RECOVERY_THRESHOLD_MAX 1950
+#endif
+
 typedef union gyroLowpassFilter_u {
     pt1Filter_t pt1FilterState;
     biquadFilter_t biquadFilterState;
@@ -98,6 +103,12 @@ enum {
     DYN_LPF_BIQUAD
 };
 
+typedef enum {
+    YAW_SPIN_RECOVERY_OFF,
+    YAW_SPIN_RECOVERY_ON,
+    YAW_SPIN_RECOVERY_AUTO
+} yawSpinRecoveryMode_e;
+
 #define GYRO_CONFIG_USE_GYRO_1      0
 #define GYRO_CONFIG_USE_GYRO_2      1
 #define GYRO_CONFIG_USE_GYRO_BOTH   2
@@ -181,3 +192,6 @@ gyroDetectionFlags_t getGyroDetectionFlags(void);
 float dynThrottle(float throttle);
 void dynLpfGyroUpdate(float throttle);
 #endif
+#ifdef USE_YAW_SPIN_RECOVERY
+void initYawSpinRecovery(int maxYawRate);
+#endif