From 41fb37a2641f72c694c72a6a9798e695b11e56e3 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Sun, 22 Apr 2018 22:45:29 +1000 Subject: [PATCH 1/2] Clean code for yaw spin recovery --- src/main/flight/mixer.c | 10 ++++++ src/main/flight/pid.c | 24 +++++++++++++ src/main/interface/settings.c | 19 +++++++---- src/main/sensors/gyro.c | 60 +++++++++++++++++++++++++++++++++ src/main/sensors/gyro.h | 4 +++ src/main/target/common_fc_pre.h | 1 + 6 files changed, 111 insertions(+), 7 deletions(-) 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 From 0a0add8c564806e05cd7a01e979c3c70862074a0 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Mon, 23 Apr 2018 20:04:39 -0400 Subject: [PATCH 2/2] 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 | 22 ++++++++++++++++------ src/main/flight/pid.c | 10 +++++++--- src/main/flight/pid.h | 2 ++ src/main/interface/settings.c | 4 ++-- src/main/sensors/gyro.c | 3 +-- 5 files changed, 28 insertions(+), 13 deletions(-) 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; }