diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d620b6a884..aec008e1e8 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -58,6 +58,7 @@ #include "rx/rx.h" #include "sensors/battery.h" +#include "sensors/gyro.h" PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0); @@ -749,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; } @@ -763,6 +775,14 @@ NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation) throttle = applyThrottleLimit(throttle); } +#ifdef USE_YAW_SPIN_RECOVERY + // 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 + // 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..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; @@ -571,6 +575,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) && yawSpinActive) { + 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 +630,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 (yawSpinActive) { + // 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 +646,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 (yawSpinActive) { + // 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/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 d5d78e299f..3a3b44f5cc 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) }, @@ -726,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 a10d7c5414..791b30cea7 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,42 @@ 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 + if (abs(gyro.gyroADCf[Z]) > gyroConfig()->yaw_spin_threshold) { + 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 +1066,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 +1247,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 4a54fa0a78..94543fafd5 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