diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 224346c85e..3060677837 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -114,7 +114,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig, #define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes) -PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 5); +PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 6); void resetPidProfile(pidProfile_t *pidProfile) { @@ -179,7 +179,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .launchControlGain = 40, .launchControlAllowTriggerReset = true, .use_integrated_yaw = false, - .integrated_yaw_relax = 200 + .integrated_yaw_relax = 200, ); #ifdef USE_DYN_LPF pidProfile->dterm_lowpass_hz = 150; @@ -438,8 +438,11 @@ static FAST_RAM_ZERO_INIT uint8_t launchControlMode; static FAST_RAM_ZERO_INIT uint8_t launchControlAngleLimit; static FAST_RAM_ZERO_INIT float launchControlKi; #endif + +#ifdef USE_INTEGRATED_YAW_CONTROL static FAST_RAM_ZERO_INIT bool useIntegratedYaw; static FAST_RAM_ZERO_INIT uint8_t integratedYawRelax; +#endif void pidResetIterm(void) { @@ -577,8 +580,11 @@ void pidInitConfig(const pidProfile_t *pidProfile) } launchControlKi = ITERM_SCALE * pidProfile->launchControlGain; #endif + +#ifdef USE_INTEGRATED_YAW_CONTROL useIntegratedYaw = pidProfile->use_integrated_yaw; integratedYawRelax = pidProfile->integrated_yaw_relax; +#endif } void pidInit(const pidProfile_t *pidProfile) @@ -1248,10 +1254,13 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT #endif // calculating the PID sum const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F; +#ifdef USE_INTEGRATED_YAW_CONTROL 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 { + } else +#endif + { pidData[axis].Sum = pidSum; } } diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 84f0dee327..7bcfa0a501 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -936,8 +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 +#ifdef USE_INTEGRATED_YAW_CONTROL { "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) }, +#endif #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) }, diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index f8fdc53c3a..725ccf4ae3 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -196,6 +196,7 @@ #define USE_RC_SMOOTHING_FILTER #define USE_ITERM_RELAX #define USE_DYN_LPF +#define USE_INTEGRATED_YAW_CONTROL #ifdef USE_SERIALRX_SPEKTRUM #define USE_SPEKTRUM_BIND