diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 407aa0d37b..c5e0bff5d1 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -569,6 +569,7 @@ const clivalue_t valueTable[] = { { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_lpf_hz) }, { "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) }, + { "gyro_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, gyroRateLimitYaw) }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].P) }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].I) }, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 13b0cf1bdb..76c4017c81 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -114,7 +114,8 @@ void resetPidProfile(pidProfile_t *pidProfile) .crash_setpoint_threshold = 350, // degrees/second .crash_recovery = PID_CRASH_RECOVERY_OFF, // off by default .horizon_tilt_effect = 75, - .horizon_tilt_expert_mode = false + .horizon_tilt_expert_mode = false, + .gyroRateLimitYaw = 1000 ); } @@ -238,6 +239,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) static float Kp[3], Ki[3], Kd[3]; static float maxVelocity[3]; +static float gyroRateLimitYaw; static float relaxFactor; static float dtermSetpointWeight; static float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio; @@ -258,6 +260,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) Ki[axis] = ITERM_SCALE * pidProfile->pid[axis].I; Kd[axis] = DTERM_SCALE * pidProfile->pid[axis].D; } + gyroRateLimitYaw = pidProfile->gyroRateLimitYaw; dtermSetpointWeight = pidProfile->dtermSetpointWeight / 127.0f; relaxFactor = 1.0f / (pidProfile->setpointRelaxRatio / 100.0f); levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f; @@ -419,7 +422,10 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an BEEP_OFF; } } - const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec + float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec + if (axis == FD_YAW) { + gyroRate = constrainf(gyroRate, -gyroRateLimitYaw, gyroRateLimitYaw); + } // --------low-level gyro-based PID based on 2DOF PID controller. ---------- // 2-DOF PID controller with optional filter on derivative term. diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index ed887dfd73..b61c5b3d5d 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -105,6 +105,7 @@ typedef struct pidProfile_s { uint8_t crash_recovery_angle; // degrees uint8_t crash_recovery_rate; // degree/second pidCrashRecovery_e crash_recovery; // off, on, on and beeps when it is in crash recovery mode + uint16_t gyroRateLimitYaw; // limits yaw gyroRate, so crashes don't cause huge throttle increase } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);