diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d620b6a884..8d094b5317 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -59,6 +59,8 @@ #include "sensors/battery.h" +#include "sensors/gyro.h" + PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0); #ifndef TARGET_DEFAULT_MIXER @@ -763,6 +765,14 @@ 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; + } +#endif // USE_YAW_SPIN_RECOVERY + // Find roll/pitch/yaw desired output float motorMix[MAX_SUPPORTED_MOTORS]; float motorMixMax = 0, motorMixMin = 0; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 00171309fc..d2e57eb631 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -571,6 +571,14 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint); } + // 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()) { + currentPidSetpoint = 0.0f; + } +#endif // USE_YAW_SPIN_RECOVERY + // -----calculate error rate const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec float errorRate = currentPidSetpoint - gyroRate; // r - y @@ -618,6 +626,15 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate); pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor; + +#ifdef USE_YAW_SPIN_RECOVERY + if (gyroYawSpinDetected()) { + // zero PIDs on pitch and roll leaving yaw P to correct spin + pidData[axis].P = 0; + pidData[axis].I = 0; + pidData[axis].D = 0; + } +#endif // USE_YAW_SPIN_RECOVERY } } @@ -625,6 +642,13 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an pidData[FD_ROLL].Sum = pidData[FD_ROLL].P + pidData[FD_ROLL].I + pidData[FD_ROLL].D; pidData[FD_PITCH].Sum = pidData[FD_PITCH].P + pidData[FD_PITCH].I + pidData[FD_PITCH].D; +#ifdef USE_YAW_SPIN_RECOVERY + if (gyroYawSpinDetected()) { + // yaw P alone to correct spin + pidData[FD_YAW].I = 0; + } +#endif // USE_YAW_SPIN_RECOVERY + // YAW has no D pidData[FD_YAW].Sum = pidData[FD_YAW].P + pidData[FD_YAW].I; diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 790a7679ff..93c2d17914 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -412,12 +412,12 @@ const clivalue_t valueTable[] = { #endif { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 32 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_sync_denom) }, - { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_type) }, - { "gyro_lowpass_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_hz) }, - { "gyro_lowpass_order", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, GYRO_LPF_ORDER_MAX}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_order) }, + { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_type) }, + { "gyro_lowpass_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_hz) }, + { "gyro_lowpass_order", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, GYRO_LPF_ORDER_MAX}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_order) }, { "gyro_lowpass2_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_type) }, - { "gyro_lowpass2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_hz) }, + { "gyro_lowpass2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_hz) }, { "gyro_lowpass2_order", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, GYRO_LPF_ORDER_MAX}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_order) }, { "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_1) }, @@ -425,14 +425,19 @@ const clivalue_t valueTable[] = { { "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_2) }, { "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) }, - { "gyro_lma_depth", VAR_UINT8 | MASTER_VALUE, .config.minmax = {0, 11}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lma_depth)}, - { "gyro_lma_weight", VAR_UINT8 | MASTER_VALUE, .config.minmax = {0, 100}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lma_weight)}, + { "gyro_lma_depth", VAR_UINT8 | MASTER_VALUE, .config.minmax = {0, 11}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lma_depth)}, + { "gyro_lma_weight", VAR_UINT8 | MASTER_VALUE, .config.minmax = {0, 100}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lma_weight)}, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) }, - { "gyro_offset_yaw", VAR_INT16 | MASTER_VALUE, .config.minmax = { -1000, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_offset_yaw) }, + { "gyro_offset_yaw", VAR_INT16 | MASTER_VALUE, .config.minmax = { -1000, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_offset_yaw) }, #ifdef USE_GYRO_OVERFLOW_CHECK { "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.minmax = { 500, 1950 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, yaw_spin_threshold) }, +#endif + #if defined(GYRO_USES_SPI) #ifdef USE_32K_CAPABLE_GYRO { "gyro_use_32khz", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_use_32khz) }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index a10d7c5414..2b24d178e5 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -138,8 +138,14 @@ typedef struct gyroSensor_s { filterApplyFnPtr notchFilterDynApplyFn; biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT]; + // overflow and recovery timeUs_t overflowTimeUs; bool overflowDetected; +#ifdef USE_YAW_SPIN_RECOVERY + timeUs_t yawSpinTimeUs; + bool yawSpinDetected; +#endif // USE_YAW_SPIN_RECOVERY + } gyroSensor_t; STATIC_UNIT_TESTED FAST_RAM gyroSensor_t gyroSensor1; @@ -202,6 +208,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_offset_yaw = 0, .gyro_lma_depth = 0, .gyro_lma_weight = 100, + .yaw_spin_recovery = true, + .yaw_spin_threshold = 1950, ); @@ -970,6 +978,9 @@ static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) if (overflowCheck & overflowAxisMask) { gyroSensor->overflowDetected = true; gyroSensor->overflowTimeUs = currentTimeUs; +#ifdef USE_YAW_SPIN_RECOVERY + gyroSensor->yawSpinDetected = false; +#endif // USE_YAW_SPIN_RECOVERY } #endif // SIMULATOR_BUILD } @@ -979,6 +990,43 @@ static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) #endif // USE_GYRO_OVERFLOW_CHECK } +static void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) +{ +#ifdef USE_YAW_SPIN_RECOVERY + // if not in overflow mode, handle yaw spins above threshold +#ifdef USE_GYRO_OVERFLOW_CHECK + if (gyroSensor->overflowDetected) { + gyroSensor->yawSpinDetected = false; + return; + } +#endif // USE_GYRO_OVERFLOW_CHECK + if (gyroSensor->yawSpinDetected) { + const float yawSpinResetRate = gyroConfig()->yaw_spin_threshold - 100.0f; + if (abs(gyro.gyroADCf[Z]) < yawSpinResetRate) { + // testing whether 20ms of consecutive OK gyro yaw values is enough + if (cmpTimeUs(currentTimeUs, gyroSensor->yawSpinTimeUs) > 20000) { + gyroSensor->yawSpinDetected = false; + } + } else { + // reset the yaw spin time + gyroSensor->yawSpinTimeUs = 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) { + gyroSensor->yawSpinDetected = true; + gyroSensor->yawSpinTimeUs = currentTimeUs; + } +#endif // SIMULATOR_BUILD + } +#else + UNUSED(gyroSensor); + UNUSED(currentTimeUs); +#endif // USE_YAW_SPIN_RECOVERY +} + static FAST_CODE NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) { if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) { @@ -1019,6 +1067,11 @@ static FAST_CODE NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs if (gyroConfig()->checkOverflow && !gyroHasOverflowProtection) { checkForOverflow(gyroSensor, currentTimeUs); } + + if (gyroConfig()->yaw_spin_recovery) { + checkForYawSpin(gyroSensor, currentTimeUs); + } + if (gyroDebugMode == DEBUG_NONE) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { // NOTE: this branch optimized for when there is no gyro debugging, ensure it is kept in step with non-optimized branch @@ -1195,6 +1248,13 @@ bool gyroOverflowDetected(void) return gyroSensor1.overflowDetected; } +#ifdef USE_YAW_SPIN_RECOVERY +bool gyroYawSpinDetected(void) +{ + return gyroSensor1.yawSpinDetected; +} +#endif // USE_YAW_SPIN_RECOVERY + uint16_t gyroAbsRateDps(int axis) { return fabsf(gyro.gyroADCf[axis]); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 37223d05ee..41a5e9c65c 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -101,6 +101,9 @@ typedef struct gyroConfig_s { gyroOverflowCheck_e checkOverflow; int16_t gyro_offset_yaw; + bool yaw_spin_recovery; + int16_t yaw_spin_threshold; + } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); @@ -122,5 +125,6 @@ void gyroReadTemperature(void); int16_t gyroGetTemperature(void); int16_t gyroRateDps(int axis); bool gyroOverflowDetected(void); +bool gyroYawSpinDetected(void); uint16_t gyroAbsRateDps(int axis); uint8_t gyroReadRegister(uint8_t reg); diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index d1c8c729fd..e741313769 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -160,6 +160,7 @@ #define USE_EXTENDED_CMS_MENUS #define USE_DSHOT_DMAR #define USE_GYRO_OVERFLOW_CHECK +#define USE_YAW_SPIN_RECOVERY #define USE_HUFFMAN #define USE_MSP_DISPLAYPORT #define USE_MSP_OVER_TELEMETRY