diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9857a899ab..224346c85e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -178,6 +178,8 @@ void resetPidProfile(pidProfile_t *pidProfile) .launchControlAngleLimit = 0, .launchControlGain = 40, .launchControlAllowTriggerReset = true, + .use_integrated_yaw = false, + .integrated_yaw_relax = 200 ); #ifdef USE_DYN_LPF pidProfile->dterm_lowpass_hz = 150; @@ -436,6 +438,8 @@ static FAST_RAM_ZERO_INIT uint8_t launchControlMode; static FAST_RAM_ZERO_INIT uint8_t launchControlAngleLimit; static FAST_RAM_ZERO_INIT float launchControlKi; #endif +static FAST_RAM_ZERO_INIT bool useIntegratedYaw; +static FAST_RAM_ZERO_INIT uint8_t integratedYawRelax; void pidResetIterm(void) { @@ -573,6 +577,8 @@ void pidInitConfig(const pidProfile_t *pidProfile) } launchControlKi = ITERM_SCALE * pidProfile->launchControlGain; #endif + useIntegratedYaw = pidProfile->use_integrated_yaw; + integratedYawRelax = pidProfile->integrated_yaw_relax; } void pidInit(const pidProfile_t *pidProfile) @@ -1241,7 +1247,13 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT } #endif // calculating the PID sum - pidData[axis].Sum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F; + const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F; + if (axis == FD_YAW && useIntegratedYaw) { + pidData[axis].Sum += pidSum * dT * 100.0f; + pidData[axis].Sum -= pidData[axis].Sum * integratedYawRelax / 100000.0f * dT / 0.000125f; + } else { + pidData[axis].Sum = pidSum; + } } // Disable PID control if at zero throttle or if gyro overflow detected diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 89afb0c65d..1287980bab 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -156,7 +156,10 @@ typedef struct pidProfile_s { uint8_t launchControlAngleLimit; // Optional launch control angle limit (requires ACC) uint8_t launchControlGain; // Iterm gain used while launch control is active uint8_t launchControlAllowTriggerReset; // Controls trigger behavior and whether the trigger can be reset + uint8_t use_integrated_yaw; // Selects whether the yaw pidsum should integrated + uint8_t integrated_yaw_relax; // Specifies how much integrated yaw should be reduced to offset the drag based yaw component } pidProfile_t; + PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index bef291c7e7..e1e68c9f6a 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -936,6 +936,10 @@ const clivalue_t valueTable[] = { { "abs_control_error_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 45 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_error_limit) }, #endif + { "use_integrated_yaw", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, use_integrated_yaw) }, + { "integrated_yaw_relax", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, integrated_yaw_relax) }, + + #ifdef USE_LAUNCH_CONTROL { "launch_control_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LAUNCH_CONTROL_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlMode) }, { "launch_trigger_allow_reset", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlAllowTriggerReset) },