diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 932c1318cb..9e2c496138 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -179,6 +179,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; @@ -448,6 +450,11 @@ 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) { for (int axis = 0; axis < 3; axis++) { @@ -585,6 +592,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) @@ -1244,7 +1256,16 @@ 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; +#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 +#endif + { + 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 37a9c41110..51bbc76920 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -157,6 +157,8 @@ 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 0aceb48ed9..01f34ef427 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -937,6 +937,11 @@ const clivalue_t valueTable[] = { { "abs_control_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 45 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_cutoff) }, #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) }, { "launch_trigger_allow_reset", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlAllowTriggerReset) }, diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index f76743d5ea..7cdc119313 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